non-causal comb filter
[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-256+st->windowSize, 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 + 50)*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 *ak);
1276    VARDECL(spx_coef_t *awk1);
1277    VARDECL(spx_coef_t *awk2);
1278    VARDECL(spx_coef_t *awk3);
1279    spx_word16_t pitch_average=0;
1280 #ifdef EPIC_48K
1281    int pitch_half[2];
1282    int ol_pitch_id=0;
1283 #endif
1284    spx_word16_t *out = vout;
1285
1286    st=(DecState*)state;
1287    stack=st->stack;
1288
1289    /* Check if we're in DTX mode*/
1290    if (!bits && st->dtx_enabled)
1291    {
1292       st->submodeID=0;
1293    } else 
1294    {
1295       /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1296       if (!bits)
1297       {
1298          nb_decode_lost(st, out, stack);
1299          return 0;
1300       }
1301
1302       if (st->encode_submode)
1303       {
1304 #ifdef EPIC_48K
1305          if (!st->lbr_48k) {
1306 #endif
1307
1308       /* Search for next narrowband block (handle requests, skip wideband blocks) */
1309       do {
1310          if (speex_bits_remaining(bits)<5)
1311             return -1;
1312          wideband = speex_bits_unpack_unsigned(bits, 1);
1313          if (wideband) /* Skip wideband block (for compatibility) */
1314          {
1315             int submode;
1316             int advance;
1317             advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1318             speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1319             if (advance < 0)
1320             {
1321                speex_warning ("Invalid wideband mode encountered. Corrupted stream?");
1322                return -2;
1323             } 
1324             advance -= (SB_SUBMODE_BITS+1);
1325             speex_bits_advance(bits, advance);
1326             
1327             if (speex_bits_remaining(bits)<5)
1328                return -1;
1329             wideband = speex_bits_unpack_unsigned(bits, 1);
1330             if (wideband)
1331             {
1332                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1333                speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1334                if (advance < 0)
1335                {
1336                   speex_warning ("Invalid wideband mode encountered: corrupted stream?");
1337                   return -2;
1338                } 
1339                advance -= (SB_SUBMODE_BITS+1);
1340                speex_bits_advance(bits, advance);
1341                wideband = speex_bits_unpack_unsigned(bits, 1);
1342                if (wideband)
1343                {
1344                   speex_warning ("More than two wideband layers found: corrupted stream?");
1345                   return -2;
1346                }
1347
1348             }
1349          }
1350          if (speex_bits_remaining(bits)<4)
1351             return -1;
1352          /* FIXME: Check for overflow */
1353          m = speex_bits_unpack_unsigned(bits, 4);
1354          if (m==15) /* We found a terminator */
1355          {
1356             return -1;
1357          } else if (m==14) /* Speex in-band request */
1358          {
1359             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1360             if (ret)
1361                return ret;
1362          } else if (m==13) /* User in-band request */
1363          {
1364             int ret = st->user_callback.func(bits, state, st->user_callback.data);
1365             if (ret)
1366                return ret;
1367          } else if (m>8) /* Invalid mode */
1368          {
1369             speex_warning("Invalid mode encountered: corrupted stream?");
1370             return -2;
1371          }
1372       
1373       } while (m>8);
1374
1375       /* Get the sub-mode that was used */
1376       st->submodeID = m;
1377 #ifdef EPIC_48K
1378          }
1379 #endif
1380       }
1381
1382    }
1383
1384    /* Shift all buffers by one frame */
1385    speex_move(st->excBuf, st->excBuf+st->frameSize, (PITCH_PERIODS*st->max_pitch + 1)*sizeof(spx_sig_t));
1386
1387    /* If null mode (no transmission), just set a couple things to zero*/
1388    if (st->submodes[st->submodeID] == NULL)
1389    {
1390       VARDECL(spx_coef_t *lpc);
1391       ALLOC(lpc, st->lpcSize, spx_coef_t);
1392       bw_lpc(GAMMA_SCALING*.93, st->interp_qlpc, lpc, st->lpcSize);
1393       {
1394          float innov_gain=0;
1395          float pgain=GAIN_SCALING_1*st->last_pitch_gain;
1396          if (pgain>.6)
1397             pgain=.6;
1398          innov_gain = compute_rms(st->innov, st->frameSize);
1399          for (i=0;i<st->frameSize;i++)
1400             st->exc[i]=VERY_SMALL;
1401          speex_rand_vec(innov_gain, st->exc, st->frameSize);
1402       }
1403
1404
1405       st->first=1;
1406
1407       /* Final signal synthesis from excitation */
1408       iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
1409
1410       for (i=0;i<st->frameSize;i++)
1411       {
1412          spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
1413          if (sig>32767)
1414             sig = 32767;
1415          if (sig<-32767)
1416             sig = -32767;
1417          out[i]=sig;
1418       }
1419
1420       st->count_lost=0;
1421       return 0;
1422    }
1423
1424    /* Unquantize LSPs */
1425    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1426
1427    /*Damp memory if a frame was lost and the LSP changed too much*/
1428    if (st->count_lost)
1429    {
1430       spx_word16_t fact;
1431       spx_word32_t lsp_dist=0;
1432       for (i=0;i<st->lpcSize;i++)
1433          lsp_dist = ADD32(lsp_dist, EXTEND32(ABS(st->old_qlsp[i] - st->qlsp[i])));
1434 #ifdef FIXED_POINT
1435       fact = SHR16(19661,SHR32(lsp_dist,LSP_SHIFT+2));      
1436 #else
1437       fact = .6*exp(-.2*lsp_dist);
1438 #endif
1439       for (i=0;i<2*st->lpcSize;i++)
1440          st->mem_sp[i] = MULT16_32_Q15(fact,st->mem_sp[i]);
1441    }
1442
1443
1444    /* Handle first frame and lost-packet case */
1445    if (st->first || st->count_lost)
1446    {
1447       for (i=0;i<st->lpcSize;i++)
1448          st->old_qlsp[i] = st->qlsp[i];
1449    }
1450
1451 #ifdef EPIC_48K
1452    if (st->lbr_48k) {
1453       pitch_half[0] = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1454       pitch_half[1] = pitch_half[0]+speex_bits_unpack_unsigned(bits, 2)-1;
1455
1456       ol_pitch_id = speex_bits_unpack_unsigned(bits, 3);
1457       ol_pitch_coef=GAIN_SCALING*0.13514*ol_pitch_id;
1458
1459       {
1460          int qe;
1461          qe = speex_bits_unpack_unsigned(bits, 4);
1462          ol_gain = SIG_SCALING*exp((qe+2)/2.1),SIG_SHIFT;
1463       }
1464
1465    } else {
1466 #endif
1467
1468    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1469    if (SUBMODE(lbr_pitch)!=-1)
1470    {
1471       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1472    } 
1473    
1474    if (SUBMODE(forced_pitch_gain))
1475    {
1476       int quant;
1477       quant = speex_bits_unpack_unsigned(bits, 4);
1478       ol_pitch_coef=GAIN_SCALING*0.066667*quant;
1479    }
1480    
1481    /* Get global excitation gain */
1482    {
1483       int qe;
1484       qe = speex_bits_unpack_unsigned(bits, 5);
1485 #ifdef FIXED_POINT
1486       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
1487 #else
1488       ol_gain = SIG_SCALING*exp(qe/3.5);
1489 #endif
1490    }
1491 #ifdef EPIC_48K
1492    }
1493 #endif
1494
1495    ALLOC(ak, st->lpcSize, spx_coef_t);
1496    ALLOC(awk1, st->lpcSize, spx_coef_t);
1497    ALLOC(awk2, st->lpcSize, spx_coef_t);
1498    ALLOC(awk3, st->lpcSize, spx_coef_t);
1499
1500    if (st->submodeID==1)
1501    {
1502       int extra;
1503       extra = speex_bits_unpack_unsigned(bits, 4);
1504
1505       if (extra==15)
1506          st->dtx_enabled=1;
1507       else
1508          st->dtx_enabled=0;
1509    }
1510    if (st->submodeID>1)
1511       st->dtx_enabled=0;
1512
1513    /*Loop on subframes */
1514    for (sub=0;sub<st->nbSubframes;sub++)
1515    {
1516       int offset;
1517       spx_sig_t *exc, *sp;
1518       spx_word16_t tmp;
1519
1520 #ifdef EPIC_48K
1521       if (st->lbr_48k)
1522       {
1523          if (sub*2 < st->nbSubframes)
1524             ol_pitch = pitch_half[0];
1525          else
1526             ol_pitch = pitch_half[1];
1527       }
1528 #endif
1529
1530       /* Offset relative to start of frame */
1531       offset = st->subframeSize*sub;
1532       /* Excitation */
1533       exc=st->exc+offset;
1534       /* Original signal */
1535       sp=st->frame+offset;
1536
1537
1538       /* Reset excitation */
1539       for (i=0;i<st->subframeSize;i++)
1540          exc[i]=0;
1541
1542       /*Adaptive codebook contribution*/
1543       if (SUBMODE(ltp_unquant))
1544       {
1545          int pit_min, pit_max;
1546          /* Handle pitch constraints if any */
1547          if (SUBMODE(lbr_pitch) != -1)
1548          {
1549             int margin;
1550             margin = SUBMODE(lbr_pitch);
1551             if (margin)
1552             {
1553 /* GT - need optimization?
1554                if (ol_pitch < st->min_pitch+margin-1)
1555                   ol_pitch=st->min_pitch+margin-1;
1556                if (ol_pitch > st->max_pitch-margin)
1557                   ol_pitch=st->max_pitch-margin;
1558                pit_min = ol_pitch-margin+1;
1559                pit_max = ol_pitch+margin;
1560 */
1561                pit_min = ol_pitch-margin+1;
1562                if (pit_min < st->min_pitch)
1563                   pit_min = st->min_pitch;
1564                pit_max = ol_pitch+margin;
1565                if (pit_max > st->max_pitch)
1566                   pit_max = st->max_pitch;
1567             } else {
1568                pit_min = pit_max = ol_pitch;
1569             }
1570          } else {
1571             pit_min = st->min_pitch;
1572             pit_max = st->max_pitch;
1573          }
1574
1575
1576 #ifdef EPIC_48K
1577          if (st->lbr_48k)
1578          {
1579              SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1580                                   st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
1581                                   st->count_lost, offset, st->last_pitch_gain, ol_pitch_id);
1582          } else {
1583 #endif
1584
1585              SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1586                                   st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
1587                                   st->count_lost, offset, st->last_pitch_gain, 0);
1588
1589 #ifdef EPIC_48K
1590          }
1591 #endif
1592
1593          
1594          /* If we had lost frames, check energy of last received frame */
1595          if (st->count_lost && ol_gain < st->last_ol_gain)
1596          {
1597             /*float fact = (float)ol_gain/(st->last_ol_gain+1);
1598             for (i=0;i<st->subframeSize;i++)
1599             exc[i]*=fact;*/
1600             spx_word16_t fact = DIV32_16(SHL32(EXTEND32(ol_gain),15),st->last_ol_gain+1);
1601             for (i=0;i<st->subframeSize;i++)
1602                exc[i] = MULT16_32_Q15(fact, exc[i]);
1603          }
1604
1605          tmp = gain_3tap_to_1tap(pitch_gain);
1606
1607          pitch_average += tmp;
1608          if (tmp>best_pitch_gain)
1609          {
1610             best_pitch = pitch;
1611             best_pitch_gain = tmp;
1612          }
1613       } else {
1614          speex_error("No pitch prediction, what's wrong");
1615       }
1616       
1617       /* Unquantize the innovation */
1618       {
1619          int q_energy;
1620          spx_word32_t ener;
1621          spx_sig_t *innov;
1622          
1623          innov = st->innov+sub*st->subframeSize;
1624          for (i=0;i<st->subframeSize;i++)
1625             innov[i]=0;
1626
1627          /* Decode sub-frame gain correction */
1628          if (SUBMODE(have_subframe_gain)==3)
1629          {
1630             q_energy = speex_bits_unpack_unsigned(bits, 3);
1631             ener = MULT16_32_Q14(exc_gain_quant_scal3[q_energy],ol_gain);
1632          } else if (SUBMODE(have_subframe_gain)==1)
1633          {
1634             q_energy = speex_bits_unpack_unsigned(bits, 1);
1635             ener = MULT16_32_Q14(exc_gain_quant_scal1[q_energy],ol_gain);
1636          } else {
1637             ener = ol_gain;
1638          }
1639                   
1640          if (SUBMODE(innovation_unquant))
1641          {
1642             /*Fixed codebook contribution*/
1643             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1644          } else {
1645             speex_error("No fixed codebook");
1646          }
1647
1648          /* De-normalize innovation and update excitation */
1649 #ifdef FIXED_POINT
1650          signal_mul(innov, innov, ener, st->subframeSize);
1651 #else
1652          signal_mul(innov, innov, ener, st->subframeSize);
1653 #endif
1654          /*Vocoder mode*/
1655          if (st->submodeID==1) 
1656          {
1657             float g=ol_pitch_coef*GAIN_SCALING_1;
1658
1659             
1660             for (i=0;i<st->subframeSize;i++)
1661                exc[i]=0;
1662             while (st->voc_offset<st->subframeSize)
1663             {
1664                if (st->voc_offset>=0)
1665                   exc[st->voc_offset]=SIG_SCALING*sqrt(1.0*ol_pitch);
1666                st->voc_offset+=ol_pitch;
1667             }
1668             st->voc_offset -= st->subframeSize;
1669
1670             g=.5+2*(g-.6);
1671             if (g<0)
1672                g=0;
1673             if (g>1)
1674                g=1;
1675             for (i=0;i<st->subframeSize;i++)
1676             {
1677                float exci=exc[i];
1678                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];
1679                st->voc_m1 = exci;
1680                st->voc_m2=innov[i];
1681                st->voc_mean = .95*st->voc_mean + .05*exc[i];
1682                exc[i]-=st->voc_mean;
1683             }
1684          } else {
1685             for (i=0;i<st->subframeSize;i++)
1686                exc[i]=ADD32(exc[i],innov[i]);
1687             /*print_vec(exc, 40, "innov");*/
1688          }
1689          /* Decode second codebook (only for some modes) */
1690          if (SUBMODE(double_codebook))
1691          {
1692             char *tmp_stack=stack;
1693             VARDECL(spx_sig_t *innov2);
1694             ALLOC(innov2, st->subframeSize, spx_sig_t);
1695             for (i=0;i<st->subframeSize;i++)
1696                innov2[i]=0;
1697             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1698             signal_mul(innov2, innov2, (spx_word32_t) (ener*(1/2.2)), st->subframeSize);
1699             for (i=0;i<st->subframeSize;i++)
1700                exc[i] = ADD32(exc[i],innov2[i]);
1701             stack = tmp_stack;
1702          }
1703
1704       }
1705
1706       /* If the last packet was lost, re-scale the excitation to obtain the same energy as encoded in ol_gain */
1707       if (st->count_lost) 
1708       {
1709          spx_word16_t exc_ener;
1710          spx_word32_t gain32;
1711          spx_word16_t gain;
1712          exc_ener = compute_rms (exc, st->subframeSize);
1713          gain32 = DIV32(ol_gain, ADD16(exc_ener,1));
1714 #ifdef FIXED_POINT
1715          if (gain32 > 32768)
1716             gain32 = 32768;
1717          gain = EXTRACT16(gain32);
1718 #else
1719          if (gain32 > 2)
1720             gain32=2;
1721          gain = gain32;
1722 #endif
1723          for (i=0;i<st->subframeSize;i++)
1724             exc[i] = MULT16_32_Q14(gain, exc[i]);
1725       }
1726
1727       for (i=0;i<st->subframeSize;i++)
1728          sp[i]=exc[i];
1729
1730       /* Signal synthesis */
1731       if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0)
1732          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1733                      pitch, pitch_gain, SUBMODE(comb_gain), st->comb_mem);
1734       
1735
1736    }
1737    
1738 #ifdef NEW_ENHANCER
1739    multicomb(st->exc-40, st->frame, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, pitch, pitch_gain, SUBMODE(comb_gain), stack);
1740    multicomb(st->exc+40, st->frame+80, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, pitch, pitch_gain, SUBMODE(comb_gain), stack);
1741 #endif
1742    /*Loop on subframes */
1743    for (sub=0;sub<st->nbSubframes;sub++)
1744    {
1745       int offset;
1746       spx_sig_t *sp, *exc;
1747       spx_word16_t tmp;
1748
1749       /* Offset relative to start of frame */
1750       offset = st->subframeSize*sub;
1751       /* Original signal */
1752       sp=st->frame+offset;
1753       /* Excitation */
1754       exc=st->exc+offset;
1755
1756       /* LSP interpolation (quantized and unquantized) */
1757       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
1758
1759       /* Make sure the LSP's are stable */
1760       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
1761
1762       /* Compute interpolated LPCs (unquantized) */
1763       lsp_to_lpc(st->interp_qlsp, ak, st->lpcSize, stack);
1764
1765 #ifndef NEW_ENHANCER
1766       for (i=0;i<st->lpcSize;i++)
1767          st->interp_qlpc[i] = ak[i];
1768 #endif
1769
1770       /* Compute enhanced synthesis filter */
1771       if (st->lpc_enh_enabled)
1772       {
1773          bw_lpc(SUBMODE(lpc_enh_k1), st->interp_qlpc, awk1, st->lpcSize);
1774          bw_lpc(SUBMODE(lpc_enh_k2), st->interp_qlpc, awk2, st->lpcSize);
1775          bw_lpc(SUBMODE(lpc_enh_k3), st->interp_qlpc, awk3, st->lpcSize);
1776       }
1777       
1778       /* Compute analysis filter at w=pi */
1779       {
1780          spx_word32_t pi_g=LPC_SCALING;
1781          for (i=0;i<st->lpcSize;i+=2)
1782          {
1783             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
1784             pi_g = ADD32(pi_g, SUB32(st->interp_qlpc[i+1],st->interp_qlpc[i]));
1785          }
1786          st->pi_gain[sub] = pi_g;
1787       }
1788       
1789       if (st->lpc_enh_enabled)
1790       {
1791          /* Use enhanced LPC filter */
1792          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1793                      st->mem_sp+st->lpcSize);
1794          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1795                      st->mem_sp);
1796       } else {
1797          /* Use regular filter */
1798          for (i=0;i<st->lpcSize;i++)
1799             st->mem_sp[st->lpcSize+i] = 0;
1800          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1801                   st->mem_sp);
1802       }
1803       
1804       for (i=0;i<st->lpcSize;i++)
1805          st->interp_qlpc[i] = ak[i];
1806
1807    }
1808    /*Copy output signal*/   
1809    for (i=0;i<st->frameSize;i++)
1810    {
1811       spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
1812       if (sig>32767)
1813          sig = 32767;
1814       if (sig<-32767)
1815          sig = -32767;
1816      out[i]=sig;
1817    }
1818
1819    /*for (i=0;i<st->frameSize;i++)
1820      printf ("%d\n", (int)st->frame[i]);*/
1821
1822    /* Store the LSPs for interpolation in the next frame */
1823    for (i=0;i<st->lpcSize;i++)
1824       st->old_qlsp[i] = st->qlsp[i];
1825
1826    /* The next frame will not be the first (Duh!) */
1827    st->first = 0;
1828    st->count_lost=0;
1829    st->last_pitch = best_pitch;
1830 #ifdef FIXED_POINT
1831    st->last_pitch_gain = PSHR16(pitch_average,2);
1832 #else
1833    st->last_pitch_gain = .25*pitch_average;   
1834 #endif
1835    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1836    if (st->pitch_gain_buf_idx > 2) /* rollover */
1837       st->pitch_gain_buf_idx = 0;
1838
1839    st->last_ol_gain = ol_gain;
1840
1841    return 0;
1842 }
1843
1844 int nb_encoder_ctl(void *state, int request, void *ptr)
1845 {
1846    EncState *st;
1847    st=(EncState*)state;     
1848    switch(request)
1849    {
1850    case SPEEX_GET_FRAME_SIZE:
1851       (*(int*)ptr) = st->frameSize;
1852       break;
1853    case SPEEX_SET_LOW_MODE:
1854    case SPEEX_SET_MODE:
1855       st->submodeSelect = st->submodeID = (*(int*)ptr);
1856       break;
1857    case SPEEX_GET_LOW_MODE:
1858    case SPEEX_GET_MODE:
1859       (*(int*)ptr) = st->submodeID;
1860       break;
1861    case SPEEX_SET_VBR:
1862       st->vbr_enabled = (*(int*)ptr);
1863       break;
1864    case SPEEX_GET_VBR:
1865       (*(int*)ptr) = st->vbr_enabled;
1866       break;
1867    case SPEEX_SET_VAD:
1868       st->vad_enabled = (*(int*)ptr);
1869       break;
1870    case SPEEX_GET_VAD:
1871       (*(int*)ptr) = st->vad_enabled;
1872       break;
1873    case SPEEX_SET_DTX:
1874       st->dtx_enabled = (*(int*)ptr);
1875       break;
1876    case SPEEX_GET_DTX:
1877       (*(int*)ptr) = st->dtx_enabled;
1878       break;
1879    case SPEEX_SET_ABR:
1880       st->abr_enabled = (*(int*)ptr);
1881       st->vbr_enabled = 1;
1882       {
1883          int i=10, rate, target;
1884          float vbr_qual;
1885          target = (*(int*)ptr);
1886          while (i>=0)
1887          {
1888             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1889             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1890             if (rate <= target)
1891                break;
1892             i--;
1893          }
1894          vbr_qual=i;
1895          if (vbr_qual<0)
1896             vbr_qual=0;
1897          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1898          st->abr_count=0;
1899          st->abr_drift=0;
1900          st->abr_drift2=0;
1901       }
1902       
1903       break;
1904    case SPEEX_GET_ABR:
1905       (*(int*)ptr) = st->abr_enabled;
1906       break;
1907    case SPEEX_SET_VBR_QUALITY:
1908       st->vbr_quality = (*(float*)ptr);
1909       break;
1910    case SPEEX_GET_VBR_QUALITY:
1911       (*(float*)ptr) = st->vbr_quality;
1912       break;
1913    case SPEEX_SET_QUALITY:
1914       {
1915          int quality = (*(int*)ptr);
1916          if (quality < 0)
1917             quality = 0;
1918          if (quality > 10)
1919             quality = 10;
1920          st->submodeSelect = st->submodeID = ((const SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1921       }
1922       break;
1923    case SPEEX_SET_COMPLEXITY:
1924       st->complexity = (*(int*)ptr);
1925       if (st->complexity<0)
1926          st->complexity=0;
1927       break;
1928    case SPEEX_GET_COMPLEXITY:
1929       (*(int*)ptr) = st->complexity;
1930       break;
1931    case SPEEX_SET_BITRATE:
1932       {
1933          int i=10, rate, target;
1934          target = (*(int*)ptr);
1935          while (i>=0)
1936          {
1937             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1938             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1939             if (rate <= target)
1940                break;
1941             i--;
1942          }
1943       }
1944       break;
1945    case SPEEX_GET_BITRATE:
1946       if (st->submodes[st->submodeID])
1947          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1948       else
1949          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1950       break;
1951    case SPEEX_SET_SAMPLING_RATE:
1952       st->sampling_rate = (*(int*)ptr);
1953       break;
1954    case SPEEX_GET_SAMPLING_RATE:
1955       (*(int*)ptr)=st->sampling_rate;
1956       break;
1957    case SPEEX_RESET_STATE:
1958       {
1959          int i;
1960          st->bounded_pitch = 1;
1961          st->first = 1;
1962          for (i=0;i<st->lpcSize;i++)
1963             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1964          for (i=0;i<st->lpcSize;i++)
1965             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1966          for (i=0;i<st->frameSize+st->max_pitch+1;i++)
1967             st->excBuf[i]=st->swBuf[i]=0;
1968          for (i=0;i<st->windowSize;i++)
1969             st->inBuf[i]=0;
1970       }
1971       break;
1972    case SPEEX_SET_SUBMODE_ENCODING:
1973       st->encode_submode = (*(int*)ptr);
1974       break;
1975    case SPEEX_GET_SUBMODE_ENCODING:
1976       (*(int*)ptr) = st->encode_submode;
1977       break;
1978    case SPEEX_GET_LOOKAHEAD:
1979       (*(int*)ptr)=(st->windowSize-st->frameSize);
1980       break;
1981    case SPEEX_SET_PLC_TUNING:
1982       st->plc_tuning = (*(int*)ptr);
1983       if (st->plc_tuning>100)
1984          st->plc_tuning=100;
1985       break;
1986    case SPEEX_GET_PLC_TUNING:
1987       (*(int*)ptr)=(st->plc_tuning);
1988       break;
1989    case SPEEX_GET_PI_GAIN:
1990       {
1991          int i;
1992          spx_word32_t *g = (spx_word32_t*)ptr;
1993          for (i=0;i<st->nbSubframes;i++)
1994             g[i]=st->pi_gain[i];
1995       }
1996       break;
1997    case SPEEX_GET_EXC:
1998       {
1999          int i;
2000          spx_sig_t *e = (spx_sig_t*)ptr;
2001          for (i=0;i<st->frameSize;i++)
2002             e[i]=st->exc[i];
2003       }
2004       break;
2005    case SPEEX_GET_INNOV:
2006       {
2007          int i;
2008          spx_sig_t *e = (spx_sig_t*)ptr;
2009          for (i=0;i<st->frameSize;i++)
2010             e[i]=st->innov[i];
2011       }
2012       break;
2013    case SPEEX_GET_RELATIVE_QUALITY:
2014       (*(float*)ptr)=st->relative_quality;
2015       break;
2016    default:
2017       speex_warning_int("Unknown nb_ctl request: ", request);
2018       return -1;
2019    }
2020    return 0;
2021 }
2022
2023 int nb_decoder_ctl(void *state, int request, void *ptr)
2024 {
2025    DecState *st;
2026    st=(DecState*)state;
2027    switch(request)
2028    {
2029    case SPEEX_SET_LOW_MODE:
2030    case SPEEX_SET_MODE:
2031       st->submodeID = (*(int*)ptr);
2032       break;
2033    case SPEEX_GET_LOW_MODE:
2034    case SPEEX_GET_MODE:
2035       (*(int*)ptr) = st->submodeID;
2036       break;
2037    case SPEEX_SET_ENH:
2038       st->lpc_enh_enabled = *((int*)ptr);
2039       break;
2040    case SPEEX_GET_ENH:
2041       *((int*)ptr) = st->lpc_enh_enabled;
2042       break;
2043    case SPEEX_GET_FRAME_SIZE:
2044       (*(int*)ptr) = st->frameSize;
2045       break;
2046    case SPEEX_GET_BITRATE:
2047       if (st->submodes[st->submodeID])
2048          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
2049       else
2050          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
2051       break;
2052    case SPEEX_SET_SAMPLING_RATE:
2053       st->sampling_rate = (*(int*)ptr);
2054       break;
2055    case SPEEX_GET_SAMPLING_RATE:
2056       (*(int*)ptr)=st->sampling_rate;
2057       break;
2058    case SPEEX_SET_HANDLER:
2059       {
2060          SpeexCallback *c = (SpeexCallback*)ptr;
2061          st->speex_callbacks[c->callback_id].func=c->func;
2062          st->speex_callbacks[c->callback_id].data=c->data;
2063          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
2064       }
2065       break;
2066    case SPEEX_SET_USER_HANDLER:
2067       {
2068          SpeexCallback *c = (SpeexCallback*)ptr;
2069          st->user_callback.func=c->func;
2070          st->user_callback.data=c->data;
2071          st->user_callback.callback_id=c->callback_id;
2072       }
2073       break;
2074    case SPEEX_RESET_STATE:
2075       {
2076          int i;
2077          for (i=0;i<2*st->lpcSize;i++)
2078             st->mem_sp[i]=0;
2079          for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
2080             st->excBuf[i]=0;
2081          for (i=0;i<st->frameSize;i++)
2082             st->inBuf[i] = 0;
2083       }
2084       break;
2085    case SPEEX_SET_SUBMODE_ENCODING:
2086       st->encode_submode = (*(int*)ptr);
2087       break;
2088    case SPEEX_GET_SUBMODE_ENCODING:
2089       (*(int*)ptr) = st->encode_submode;
2090       break;
2091    case SPEEX_GET_PI_GAIN:
2092       {
2093          int i;
2094          spx_word32_t *g = (spx_word32_t*)ptr;
2095          for (i=0;i<st->nbSubframes;i++)
2096             g[i]=st->pi_gain[i];
2097       }
2098       break;
2099    case SPEEX_GET_EXC:
2100       {
2101          int i;
2102          spx_sig_t *e = (spx_sig_t*)ptr;
2103          for (i=0;i<st->frameSize;i++)
2104             e[i]=st->exc[i];
2105       }
2106       break;
2107    case SPEEX_GET_INNOV:
2108       {
2109          int i;
2110          spx_sig_t *e = (spx_sig_t*)ptr;
2111          for (i=0;i<st->frameSize;i++)
2112             e[i]=st->innov[i];
2113       }
2114       break;
2115    case SPEEX_GET_DTX_STATUS:
2116       *((int*)ptr) = st->dtx_enabled;
2117       break;
2118    default:
2119       speex_warning_int("Unknown nb_ctl request: ", request);
2120       return -1;
2121    }
2122    return 0;
2123 }