48222f569e65bb399a4d5a1a1a03a1277e133260
[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 #define PITCH_PERIODS 1
1020
1021 void *nb_decoder_init(const SpeexMode *m)
1022 {
1023    DecState *st;
1024    const SpeexNBMode *mode;
1025    int i;
1026
1027    mode=(const SpeexNBMode*)m->mode;
1028    st = (DecState *)speex_alloc(sizeof(DecState));
1029    if (!st)
1030       return NULL;
1031 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
1032    st->stack = NULL;
1033 #else
1034    st->stack = (char*)speex_alloc_scratch(NB_DEC_STACK);
1035 #endif
1036
1037    st->mode=m;
1038
1039
1040    st->encode_submode = 1;
1041 #ifdef EPIC_48K
1042    st->lbr_48k=mode->lbr48k;
1043 #endif
1044
1045    st->first=1;
1046    /* Codec parameters, should eventually have several "modes"*/
1047    st->frameSize = mode->frameSize;
1048    st->nbSubframes=mode->frameSize/mode->subframeSize;
1049    st->subframeSize=mode->subframeSize;
1050    st->lpcSize = mode->lpcSize;
1051    st->min_pitch=mode->pitchStart;
1052    st->max_pitch=mode->pitchEnd;
1053
1054    st->submodes=mode->submodes;
1055    st->submodeID=mode->defaultSubmode;
1056
1057    st->lpc_enh_enabled=0;
1058
1059
1060    st->inBuf = speex_alloc((st->frameSize)*sizeof(spx_sig_t));
1061    st->frame = st->inBuf;
1062    st->excBuf = speex_alloc((st->frameSize + PITCH_PERIODS*st->max_pitch + 1)*sizeof(spx_sig_t));
1063    st->exc = st->excBuf + PITCH_PERIODS*st->max_pitch + 1;
1064    for (i=0;i<st->frameSize;i++)
1065       st->inBuf[i]=0;
1066    for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
1067       st->excBuf[i]=0;
1068    st->innov = speex_alloc((st->frameSize)*sizeof(spx_sig_t));
1069
1070    st->interp_qlpc = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
1071    st->qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
1072    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
1073    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
1074    st->mem_sp = speex_alloc((5*st->lpcSize)*sizeof(spx_mem_t));
1075    st->comb_mem = speex_alloc(sizeof(CombFilterMem));
1076    comb_filter_mem_init (st->comb_mem);
1077
1078    st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
1079    st->last_pitch = 40;
1080    st->count_lost=0;
1081    st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
1082    st->pitch_gain_buf_idx = 0;
1083    st->seed = 1000;
1084    
1085    st->sampling_rate=8000;
1086    st->last_ol_gain = 0;
1087
1088    st->user_callback.func = &speex_default_user_handler;
1089    st->user_callback.data = NULL;
1090    for (i=0;i<16;i++)
1091       st->speex_callbacks[i].func = NULL;
1092
1093    st->voc_m1=st->voc_m2=st->voc_mean=0;
1094    st->voc_offset=0;
1095    st->dtx_enabled=0;
1096 #ifdef ENABLE_VALGRIND
1097    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
1098 #endif
1099    return st;
1100 }
1101
1102 void nb_decoder_destroy(void *state)
1103 {
1104    DecState *st;
1105    st=(DecState*)state;
1106    
1107 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
1108    speex_free_scratch(st->stack);
1109 #endif
1110
1111    speex_free (st->inBuf);
1112    speex_free (st->excBuf);
1113    speex_free (st->innov);
1114    speex_free (st->interp_qlpc);
1115    speex_free (st->qlsp);
1116    speex_free (st->old_qlsp);
1117    speex_free (st->interp_qlsp);
1118    speex_free (st->mem_sp);
1119    speex_free (st->comb_mem);
1120    speex_free (st->pi_gain);
1121
1122    speex_free(state);
1123 }
1124
1125 #define median3(a, b, c)        ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
1126
1127 #ifdef FIXED_POINT
1128 const spx_word16_t attenuation[10] = {32767, 31483, 27923, 22861, 17278, 12055, 7764, 4616, 2533, 1283};
1129 #else
1130 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};
1131
1132 #endif
1133
1134 static void nb_decode_lost(DecState *st, spx_word16_t *out, char *stack)
1135 {
1136    int i, sub;
1137    int pitch_val;
1138    VARDECL(spx_coef_t *awk1);
1139    VARDECL(spx_coef_t *awk2);
1140    VARDECL(spx_coef_t *awk3);
1141    spx_word16_t pitch_gain;
1142    spx_word16_t fact;
1143    spx_word16_t gain_med;
1144    spx_word16_t innov_gain;
1145    
1146    if (st->count_lost<10)
1147       fact = attenuation[st->count_lost];
1148    else
1149       fact = 0;
1150
1151    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
1152    if (gain_med < st->last_pitch_gain)
1153       st->last_pitch_gain = gain_med;
1154    
1155 #ifdef FIXED_POINT
1156    pitch_gain = st->last_pitch_gain;
1157    if (pitch_gain>54)
1158       pitch_gain = 54;
1159    pitch_gain = SHL(pitch_gain, 9);
1160 #else   
1161    pitch_gain = GAIN_SCALING_1*st->last_pitch_gain;
1162    if (pitch_gain>.85)
1163       pitch_gain=.85;
1164 #endif
1165
1166    pitch_gain = MULT16_16_Q15(fact,pitch_gain) + VERY_SMALL;
1167
1168    /* Shift all buffers by one frame */
1169    /*speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(spx_sig_t));*/
1170    speex_move(st->excBuf, st->excBuf+st->frameSize, (PITCH_PERIODS*st->max_pitch + 1)*sizeof(spx_sig_t));
1171
1172    ALLOC(awk1, (st->lpcSize+1), spx_coef_t);
1173    ALLOC(awk2, (st->lpcSize+1), spx_coef_t);
1174    ALLOC(awk3, (st->lpcSize+1), spx_coef_t);
1175
1176    for (sub=0;sub<st->nbSubframes;sub++)
1177    {
1178       int offset;
1179       spx_sig_t *sp, *exc;
1180       /* Offset relative to start of frame */
1181       offset = st->subframeSize*sub;
1182       /* Original signal */
1183       sp=st->frame+offset;
1184       /* Excitation */
1185       exc=st->exc+offset;
1186       /* Excitation after post-filter*/
1187
1188       /* Calculate perceptually enhanced LPC filter */
1189       if (st->lpc_enh_enabled)
1190       {
1191          spx_word16_t k1,k2,k3;
1192          if (st->submodes[st->submodeID] != NULL)
1193          {
1194             k1=SUBMODE(lpc_enh_k1);
1195             k2=SUBMODE(lpc_enh_k2);
1196             k3=SUBMODE(lpc_enh_k3);
1197          } else {
1198             k1=k2=.7*GAMMA_SCALING;
1199             k3=.0;
1200          }
1201          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1202          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1203          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1204       }
1205         
1206       /* Make up a plausible excitation */
1207       /* FIXME: THIS CAN BE IMPROVED */
1208       /*if (pitch_gain>.95)
1209         pitch_gain=.95;*/
1210       innov_gain = compute_rms(st->innov, st->frameSize);
1211       pitch_val = st->last_pitch + SHR32((spx_int32_t)speex_rand(1+st->count_lost, &st->seed),SIG_SHIFT);
1212       if (pitch_val > st->max_pitch)
1213          pitch_val = st->max_pitch;
1214       if (pitch_val < st->min_pitch)
1215          pitch_val = st->min_pitch;
1216       for (i=0;i<st->subframeSize;i++)
1217       {
1218          exc[i]= MULT16_32_Q15(pitch_gain, (exc[i-pitch_val]+VERY_SMALL)) + 
1219                MULT16_32_Q15(fact, MULT16_32_Q15(SHL(Q15ONE,15)-SHL(MULT16_16(pitch_gain,pitch_gain),1),speex_rand(innov_gain, &st->seed)));
1220       }
1221       
1222       for (i=0;i<st->subframeSize;i++)
1223          sp[i]=exc[i];
1224       
1225       /* Signal synthesis */
1226       if (st->lpc_enh_enabled)
1227       {
1228          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1229                      st->mem_sp+st->lpcSize);
1230          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1231                      st->mem_sp);
1232       } else {
1233          for (i=0;i<st->lpcSize;i++)
1234             st->mem_sp[st->lpcSize+i] = 0;
1235          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1236                      st->mem_sp);
1237       }      
1238    }
1239
1240    for (i=0;i<st->frameSize;i++)
1241    {
1242       spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
1243       if (sig>32767)
1244          sig = 32767;
1245       if (sig<-32767)
1246          sig = -32767;
1247      out[i]=sig;
1248    }
1249    
1250    st->first = 0;
1251    st->count_lost++;
1252    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = PSHR(pitch_gain,9);
1253    if (st->pitch_gain_buf_idx > 2) /* rollover */
1254       st->pitch_gain_buf_idx = 0;
1255 }
1256
1257 int nb_decode(void *state, SpeexBits *bits, void *vout)
1258 {
1259    DecState *st;
1260    int i, sub;
1261    int pitch;
1262    spx_word16_t pitch_gain[3];
1263    spx_word32_t ol_gain=0;
1264    int ol_pitch=0;
1265    spx_word16_t ol_pitch_coef=0;
1266    int best_pitch=40;
1267    spx_word16_t best_pitch_gain=0;
1268    int wideband;
1269    int m;
1270    char *stack;
1271    VARDECL(spx_coef_t *awk1);
1272    VARDECL(spx_coef_t *awk2);
1273    VARDECL(spx_coef_t *awk3);
1274    spx_word16_t pitch_average=0;
1275 #ifdef EPIC_48K
1276    int pitch_half[2];
1277    int ol_pitch_id=0;
1278 #endif
1279    spx_word16_t *out = vout;
1280
1281    st=(DecState*)state;
1282    stack=st->stack;
1283
1284    /* Check if we're in DTX mode*/
1285    if (!bits && st->dtx_enabled)
1286    {
1287       st->submodeID=0;
1288    } else 
1289    {
1290       /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1291       if (!bits)
1292       {
1293          nb_decode_lost(st, out, stack);
1294          return 0;
1295       }
1296
1297       if (st->encode_submode)
1298       {
1299 #ifdef EPIC_48K
1300          if (!st->lbr_48k) {
1301 #endif
1302
1303       /* Search for next narrowband block (handle requests, skip wideband blocks) */
1304       do {
1305          if (speex_bits_remaining(bits)<5)
1306             return -1;
1307          wideband = speex_bits_unpack_unsigned(bits, 1);
1308          if (wideband) /* Skip wideband block (for compatibility) */
1309          {
1310             int submode;
1311             int advance;
1312             advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1313             speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1314             if (advance < 0)
1315             {
1316                speex_warning ("Invalid wideband mode encountered. Corrupted stream?");
1317                return -2;
1318             } 
1319             advance -= (SB_SUBMODE_BITS+1);
1320             speex_bits_advance(bits, advance);
1321             
1322             if (speex_bits_remaining(bits)<5)
1323                return -1;
1324             wideband = speex_bits_unpack_unsigned(bits, 1);
1325             if (wideband)
1326             {
1327                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1328                speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1329                if (advance < 0)
1330                {
1331                   speex_warning ("Invalid wideband mode encountered: corrupted stream?");
1332                   return -2;
1333                } 
1334                advance -= (SB_SUBMODE_BITS+1);
1335                speex_bits_advance(bits, advance);
1336                wideband = speex_bits_unpack_unsigned(bits, 1);
1337                if (wideband)
1338                {
1339                   speex_warning ("More than two wideband layers found: corrupted stream?");
1340                   return -2;
1341                }
1342
1343             }
1344          }
1345          if (speex_bits_remaining(bits)<4)
1346             return -1;
1347          /* FIXME: Check for overflow */
1348          m = speex_bits_unpack_unsigned(bits, 4);
1349          if (m==15) /* We found a terminator */
1350          {
1351             return -1;
1352          } else if (m==14) /* Speex in-band request */
1353          {
1354             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1355             if (ret)
1356                return ret;
1357          } else if (m==13) /* User in-band request */
1358          {
1359             int ret = st->user_callback.func(bits, state, st->user_callback.data);
1360             if (ret)
1361                return ret;
1362          } else if (m>8) /* Invalid mode */
1363          {
1364             speex_warning("Invalid mode encountered: corrupted stream?");
1365             return -2;
1366          }
1367       
1368       } while (m>8);
1369
1370       /* Get the sub-mode that was used */
1371       st->submodeID = m;
1372 #ifdef EPIC_48K
1373          }
1374 #endif
1375       }
1376
1377    }
1378
1379    /* Shift all buffers by one frame */
1380    speex_move(st->excBuf, st->excBuf+st->frameSize, (PITCH_PERIODS*st->max_pitch + 1)*sizeof(spx_sig_t));
1381
1382    /* If null mode (no transmission), just set a couple things to zero*/
1383    if (st->submodes[st->submodeID] == NULL)
1384    {
1385       VARDECL(spx_coef_t *lpc);
1386       ALLOC(lpc, st->lpcSize, spx_coef_t);
1387       bw_lpc(GAMMA_SCALING*.93, st->interp_qlpc, lpc, st->lpcSize);
1388       {
1389          float innov_gain=0;
1390          float pgain=GAIN_SCALING_1*st->last_pitch_gain;
1391          if (pgain>.6)
1392             pgain=.6;
1393          innov_gain = compute_rms(st->innov, st->frameSize);
1394          for (i=0;i<st->frameSize;i++)
1395             st->exc[i]=VERY_SMALL;
1396          speex_rand_vec(innov_gain, st->exc, st->frameSize);
1397       }
1398
1399
1400       st->first=1;
1401
1402       /* Final signal synthesis from excitation */
1403       iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
1404
1405       for (i=0;i<st->frameSize;i++)
1406       {
1407          spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
1408          if (sig>32767)
1409             sig = 32767;
1410          if (sig<-32767)
1411             sig = -32767;
1412          out[i]=sig;
1413       }
1414
1415       st->count_lost=0;
1416       return 0;
1417    }
1418
1419    /* Unquantize LSPs */
1420    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1421
1422    /*Damp memory if a frame was lost and the LSP changed too much*/
1423    if (st->count_lost)
1424    {
1425       spx_word16_t fact;
1426       spx_word32_t lsp_dist=0;
1427       for (i=0;i<st->lpcSize;i++)
1428          lsp_dist = ADD32(lsp_dist, EXTEND32(ABS(st->old_qlsp[i] - st->qlsp[i])));
1429 #ifdef FIXED_POINT
1430       fact = SHR16(19661,SHR32(lsp_dist,LSP_SHIFT+2));      
1431 #else
1432       fact = .6*exp(-.2*lsp_dist);
1433 #endif
1434       for (i=0;i<2*st->lpcSize;i++)
1435          st->mem_sp[i] = MULT16_32_Q15(fact,st->mem_sp[i]);
1436    }
1437
1438
1439    /* Handle first frame and lost-packet case */
1440    if (st->first || st->count_lost)
1441    {
1442       for (i=0;i<st->lpcSize;i++)
1443          st->old_qlsp[i] = st->qlsp[i];
1444    }
1445
1446 #ifdef EPIC_48K
1447    if (st->lbr_48k) {
1448       pitch_half[0] = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1449       pitch_half[1] = pitch_half[0]+speex_bits_unpack_unsigned(bits, 2)-1;
1450
1451       ol_pitch_id = speex_bits_unpack_unsigned(bits, 3);
1452       ol_pitch_coef=GAIN_SCALING*0.13514*ol_pitch_id;
1453
1454       {
1455          int qe;
1456          qe = speex_bits_unpack_unsigned(bits, 4);
1457          ol_gain = SIG_SCALING*exp((qe+2)/2.1),SIG_SHIFT;
1458       }
1459
1460    } else {
1461 #endif
1462
1463    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1464    if (SUBMODE(lbr_pitch)!=-1)
1465    {
1466       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1467    } 
1468    
1469    if (SUBMODE(forced_pitch_gain))
1470    {
1471       int quant;
1472       quant = speex_bits_unpack_unsigned(bits, 4);
1473       ol_pitch_coef=GAIN_SCALING*0.066667*quant;
1474    }
1475    
1476    /* Get global excitation gain */
1477    {
1478       int qe;
1479       qe = speex_bits_unpack_unsigned(bits, 5);
1480 #ifdef FIXED_POINT
1481       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
1482 #else
1483       ol_gain = SIG_SCALING*exp(qe/3.5);
1484 #endif
1485    }
1486 #ifdef EPIC_48K
1487    }
1488 #endif
1489
1490    ALLOC(awk1, st->lpcSize+1, spx_coef_t);
1491    ALLOC(awk2, st->lpcSize+1, spx_coef_t);
1492    ALLOC(awk3, st->lpcSize+1, spx_coef_t);
1493
1494    if (st->submodeID==1)
1495    {
1496       int extra;
1497       extra = speex_bits_unpack_unsigned(bits, 4);
1498
1499       if (extra==15)
1500          st->dtx_enabled=1;
1501       else
1502          st->dtx_enabled=0;
1503    }
1504    if (st->submodeID>1)
1505       st->dtx_enabled=0;
1506
1507    /*Loop on subframes */
1508    for (sub=0;sub<st->nbSubframes;sub++)
1509    {
1510       int offset;
1511       spx_sig_t *exc, *sp;
1512       spx_word16_t tmp;
1513
1514 #ifdef EPIC_48K
1515       if (st->lbr_48k)
1516       {
1517          if (sub*2 < st->nbSubframes)
1518             ol_pitch = pitch_half[0];
1519          else
1520             ol_pitch = pitch_half[1];
1521       }
1522 #endif
1523
1524       /* Offset relative to start of frame */
1525       offset = st->subframeSize*sub;
1526       /* Excitation */
1527       exc=st->exc+offset;
1528       /* Original signal */
1529       sp=st->frame+offset;
1530
1531
1532       /* Reset excitation */
1533       for (i=0;i<st->subframeSize;i++)
1534          exc[i]=0;
1535
1536       /*Adaptive codebook contribution*/
1537       if (SUBMODE(ltp_unquant))
1538       {
1539          int pit_min, pit_max;
1540          /* Handle pitch constraints if any */
1541          if (SUBMODE(lbr_pitch) != -1)
1542          {
1543             int margin;
1544             margin = SUBMODE(lbr_pitch);
1545             if (margin)
1546             {
1547 /* GT - need optimization?
1548                if (ol_pitch < st->min_pitch+margin-1)
1549                   ol_pitch=st->min_pitch+margin-1;
1550                if (ol_pitch > st->max_pitch-margin)
1551                   ol_pitch=st->max_pitch-margin;
1552                pit_min = ol_pitch-margin+1;
1553                pit_max = ol_pitch+margin;
1554 */
1555                pit_min = ol_pitch-margin+1;
1556                if (pit_min < st->min_pitch)
1557                   pit_min = st->min_pitch;
1558                pit_max = ol_pitch+margin;
1559                if (pit_max > st->max_pitch)
1560                   pit_max = st->max_pitch;
1561             } else {
1562                pit_min = pit_max = ol_pitch;
1563             }
1564          } else {
1565             pit_min = st->min_pitch;
1566             pit_max = st->max_pitch;
1567          }
1568
1569
1570 #ifdef EPIC_48K
1571          if (st->lbr_48k)
1572          {
1573              SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1574                                   st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
1575                                   st->count_lost, offset, st->last_pitch_gain, ol_pitch_id);
1576          } else {
1577 #endif
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, 0);
1582
1583 #ifdef EPIC_48K
1584          }
1585 #endif
1586
1587          
1588          /* If we had lost frames, check energy of last received frame */
1589          if (st->count_lost && ol_gain < st->last_ol_gain)
1590          {
1591             /*float fact = (float)ol_gain/(st->last_ol_gain+1);
1592             for (i=0;i<st->subframeSize;i++)
1593             exc[i]*=fact;*/
1594             spx_word16_t fact = DIV32_16(SHL32(EXTEND32(ol_gain),15),st->last_ol_gain+1);
1595             for (i=0;i<st->subframeSize;i++)
1596                exc[i] = MULT16_32_Q15(fact, exc[i]);
1597          }
1598
1599          tmp = gain_3tap_to_1tap(pitch_gain);
1600
1601          pitch_average += tmp;
1602          if (tmp>best_pitch_gain)
1603          {
1604             best_pitch = pitch;
1605             best_pitch_gain = tmp;
1606          }
1607       } else {
1608          speex_error("No pitch prediction, what's wrong");
1609       }
1610       
1611       /* Unquantize the innovation */
1612       {
1613          int q_energy;
1614          spx_word32_t ener;
1615          spx_sig_t *innov;
1616          
1617          innov = st->innov+sub*st->subframeSize;
1618          for (i=0;i<st->subframeSize;i++)
1619             innov[i]=0;
1620
1621          /* Decode sub-frame gain correction */
1622          if (SUBMODE(have_subframe_gain)==3)
1623          {
1624             q_energy = speex_bits_unpack_unsigned(bits, 3);
1625             ener = MULT16_32_Q14(exc_gain_quant_scal3[q_energy],ol_gain);
1626          } else if (SUBMODE(have_subframe_gain)==1)
1627          {
1628             q_energy = speex_bits_unpack_unsigned(bits, 1);
1629             ener = MULT16_32_Q14(exc_gain_quant_scal1[q_energy],ol_gain);
1630          } else {
1631             ener = ol_gain;
1632          }
1633                   
1634          if (SUBMODE(innovation_unquant))
1635          {
1636             /*Fixed codebook contribution*/
1637             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1638          } else {
1639             speex_error("No fixed codebook");
1640          }
1641
1642          /* De-normalize innovation and update excitation */
1643 #ifdef FIXED_POINT
1644          signal_mul(innov, innov, ener, st->subframeSize);
1645 #else
1646          signal_mul(innov, innov, ener, st->subframeSize);
1647 #endif
1648          /*Vocoder mode*/
1649          if (st->submodeID==1) 
1650          {
1651             float g=ol_pitch_coef*GAIN_SCALING_1;
1652
1653             
1654             for (i=0;i<st->subframeSize;i++)
1655                exc[i]=0;
1656             while (st->voc_offset<st->subframeSize)
1657             {
1658                if (st->voc_offset>=0)
1659                   exc[st->voc_offset]=SIG_SCALING*sqrt(1.0*ol_pitch);
1660                st->voc_offset+=ol_pitch;
1661             }
1662             st->voc_offset -= st->subframeSize;
1663
1664             g=.5+2*(g-.6);
1665             if (g<0)
1666                g=0;
1667             if (g>1)
1668                g=1;
1669             for (i=0;i<st->subframeSize;i++)
1670             {
1671                float exci=exc[i];
1672                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];
1673                st->voc_m1 = exci;
1674                st->voc_m2=innov[i];
1675                st->voc_mean = .95*st->voc_mean + .05*exc[i];
1676                exc[i]-=st->voc_mean;
1677             }
1678          } else {
1679             for (i=0;i<st->subframeSize;i++)
1680                exc[i]=ADD32(exc[i],innov[i]);
1681             /*print_vec(exc, 40, "innov");*/
1682          }
1683          /* Decode second codebook (only for some modes) */
1684          if (SUBMODE(double_codebook))
1685          {
1686             char *tmp_stack=stack;
1687             VARDECL(spx_sig_t *innov2);
1688             ALLOC(innov2, st->subframeSize, spx_sig_t);
1689             for (i=0;i<st->subframeSize;i++)
1690                innov2[i]=0;
1691             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1692             signal_mul(innov2, innov2, (spx_word32_t) (ener*(1/2.2)), st->subframeSize);
1693             for (i=0;i<st->subframeSize;i++)
1694                exc[i] = ADD32(exc[i],innov2[i]);
1695             stack = tmp_stack;
1696          }
1697
1698       }
1699
1700       /* If the last packet was lost, re-scale the excitation to obtain the same energy as encoded in ol_gain */
1701       if (st->count_lost) 
1702       {
1703          spx_word16_t exc_ener;
1704          spx_word32_t gain32;
1705          spx_word16_t gain;
1706          exc_ener = compute_rms (exc, st->subframeSize);
1707          gain32 = DIV32(ol_gain, ADD16(exc_ener,1));
1708 #ifdef FIXED_POINT
1709          if (gain32 > 32768)
1710             gain32 = 32768;
1711          gain = EXTRACT16(gain32);
1712 #else
1713          if (gain32 > 2)
1714             gain32=2;
1715          gain = gain32;
1716 #endif
1717          for (i=0;i<st->subframeSize;i++)
1718             exc[i] = MULT16_32_Q14(gain, exc[i]);
1719       }
1720
1721       for (i=0;i<st->subframeSize;i++)
1722          sp[i]=exc[i];
1723
1724       /* Signal synthesis */
1725       if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0)
1726          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1727                      pitch, pitch_gain, SUBMODE(comb_gain), st->comb_mem);
1728       
1729
1730    }
1731    
1732 #if 0 /* Disabled for now */
1733    if (1)
1734    {
1735       multicomb(st->exc, st->frame, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, pitch, pitch_gain, SUBMODE(comb_gain), stack);
1736       multicomb(st->exc+80, st->frame+80, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, pitch, pitch_gain, SUBMODE(comb_gain), stack);
1737    }
1738 #endif
1739
1740    /*Loop on subframes */
1741    for (sub=0;sub<st->nbSubframes;sub++)
1742    {
1743       int offset;
1744       spx_sig_t *sp, *exc;
1745       spx_word16_t tmp;
1746
1747       /* Offset relative to start of frame */
1748       offset = st->subframeSize*sub;
1749       /* Original signal */
1750       sp=st->frame+offset;
1751       /* Excitation */
1752       exc=st->exc+offset;
1753
1754       /* LSP interpolation (quantized and unquantized) */
1755       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
1756
1757       /* Make sure the LSP's are stable */
1758       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
1759
1760
1761       /* Compute interpolated LPCs (unquantized) */
1762       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
1763
1764       /* Compute enhanced synthesis filter */
1765       if (st->lpc_enh_enabled)
1766       {
1767          bw_lpc(SUBMODE(lpc_enh_k1), st->interp_qlpc, awk1, st->lpcSize);
1768          bw_lpc(SUBMODE(lpc_enh_k2), st->interp_qlpc, awk2, st->lpcSize);
1769          bw_lpc(SUBMODE(lpc_enh_k3), st->interp_qlpc, awk3, st->lpcSize);
1770       }
1771       
1772       /* Compute analysis filter at w=pi */
1773       {
1774          spx_word32_t pi_g=LPC_SCALING;
1775          for (i=0;i<st->lpcSize;i+=2)
1776          {
1777             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
1778             pi_g = ADD32(pi_g, SUB32(st->interp_qlpc[i+1],st->interp_qlpc[i]));
1779          }
1780          st->pi_gain[sub] = pi_g;
1781       }
1782       
1783       if (st->lpc_enh_enabled)
1784       {
1785          /* Use enhanced LPC filter */
1786          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1787                      st->mem_sp+st->lpcSize);
1788          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1789                      st->mem_sp);
1790       } else {
1791          /* Use regular filter */
1792          for (i=0;i<st->lpcSize;i++)
1793             st->mem_sp[st->lpcSize+i] = 0;
1794          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1795                   st->mem_sp);
1796       }
1797
1798    }
1799    /*Copy output signal*/   
1800    for (i=0;i<st->frameSize;i++)
1801    {
1802       spx_word32_t sig = PSHR32(st->frame[i],SIG_SHIFT);
1803       if (sig>32767)
1804          sig = 32767;
1805       if (sig<-32767)
1806          sig = -32767;
1807      out[i]=sig;
1808    }
1809
1810    /*for (i=0;i<st->frameSize;i++)
1811      printf ("%d\n", (int)st->frame[i]);*/
1812
1813    /* Store the LSPs for interpolation in the next frame */
1814    for (i=0;i<st->lpcSize;i++)
1815       st->old_qlsp[i] = st->qlsp[i];
1816
1817    /* The next frame will not be the first (Duh!) */
1818    st->first = 0;
1819    st->count_lost=0;
1820    st->last_pitch = best_pitch;
1821 #ifdef FIXED_POINT
1822    st->last_pitch_gain = PSHR16(pitch_average,2);
1823 #else
1824    st->last_pitch_gain = .25*pitch_average;   
1825 #endif
1826    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1827    if (st->pitch_gain_buf_idx > 2) /* rollover */
1828       st->pitch_gain_buf_idx = 0;
1829
1830    st->last_ol_gain = ol_gain;
1831
1832    return 0;
1833 }
1834
1835 int nb_encoder_ctl(void *state, int request, void *ptr)
1836 {
1837    EncState *st;
1838    st=(EncState*)state;     
1839    switch(request)
1840    {
1841    case SPEEX_GET_FRAME_SIZE:
1842       (*(int*)ptr) = st->frameSize;
1843       break;
1844    case SPEEX_SET_LOW_MODE:
1845    case SPEEX_SET_MODE:
1846       st->submodeSelect = st->submodeID = (*(int*)ptr);
1847       break;
1848    case SPEEX_GET_LOW_MODE:
1849    case SPEEX_GET_MODE:
1850       (*(int*)ptr) = st->submodeID;
1851       break;
1852    case SPEEX_SET_VBR:
1853       st->vbr_enabled = (*(int*)ptr);
1854       break;
1855    case SPEEX_GET_VBR:
1856       (*(int*)ptr) = st->vbr_enabled;
1857       break;
1858    case SPEEX_SET_VAD:
1859       st->vad_enabled = (*(int*)ptr);
1860       break;
1861    case SPEEX_GET_VAD:
1862       (*(int*)ptr) = st->vad_enabled;
1863       break;
1864    case SPEEX_SET_DTX:
1865       st->dtx_enabled = (*(int*)ptr);
1866       break;
1867    case SPEEX_GET_DTX:
1868       (*(int*)ptr) = st->dtx_enabled;
1869       break;
1870    case SPEEX_SET_ABR:
1871       st->abr_enabled = (*(int*)ptr);
1872       st->vbr_enabled = 1;
1873       {
1874          int i=10, rate, target;
1875          float vbr_qual;
1876          target = (*(int*)ptr);
1877          while (i>=0)
1878          {
1879             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1880             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1881             if (rate <= target)
1882                break;
1883             i--;
1884          }
1885          vbr_qual=i;
1886          if (vbr_qual<0)
1887             vbr_qual=0;
1888          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1889          st->abr_count=0;
1890          st->abr_drift=0;
1891          st->abr_drift2=0;
1892       }
1893       
1894       break;
1895    case SPEEX_GET_ABR:
1896       (*(int*)ptr) = st->abr_enabled;
1897       break;
1898    case SPEEX_SET_VBR_QUALITY:
1899       st->vbr_quality = (*(float*)ptr);
1900       break;
1901    case SPEEX_GET_VBR_QUALITY:
1902       (*(float*)ptr) = st->vbr_quality;
1903       break;
1904    case SPEEX_SET_QUALITY:
1905       {
1906          int quality = (*(int*)ptr);
1907          if (quality < 0)
1908             quality = 0;
1909          if (quality > 10)
1910             quality = 10;
1911          st->submodeSelect = st->submodeID = ((const SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1912       }
1913       break;
1914    case SPEEX_SET_COMPLEXITY:
1915       st->complexity = (*(int*)ptr);
1916       if (st->complexity<0)
1917          st->complexity=0;
1918       break;
1919    case SPEEX_GET_COMPLEXITY:
1920       (*(int*)ptr) = st->complexity;
1921       break;
1922    case SPEEX_SET_BITRATE:
1923       {
1924          int i=10, rate, target;
1925          target = (*(int*)ptr);
1926          while (i>=0)
1927          {
1928             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1929             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1930             if (rate <= target)
1931                break;
1932             i--;
1933          }
1934       }
1935       break;
1936    case SPEEX_GET_BITRATE:
1937       if (st->submodes[st->submodeID])
1938          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1939       else
1940          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1941       break;
1942    case SPEEX_SET_SAMPLING_RATE:
1943       st->sampling_rate = (*(int*)ptr);
1944       break;
1945    case SPEEX_GET_SAMPLING_RATE:
1946       (*(int*)ptr)=st->sampling_rate;
1947       break;
1948    case SPEEX_RESET_STATE:
1949       {
1950          int i;
1951          st->bounded_pitch = 1;
1952          st->first = 1;
1953          for (i=0;i<st->lpcSize;i++)
1954             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1955          for (i=0;i<st->lpcSize;i++)
1956             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1957          for (i=0;i<st->frameSize+st->max_pitch+1;i++)
1958             st->excBuf[i]=st->swBuf[i]=0;
1959          for (i=0;i<st->windowSize;i++)
1960             st->inBuf[i]=0;
1961       }
1962       break;
1963    case SPEEX_SET_SUBMODE_ENCODING:
1964       st->encode_submode = (*(int*)ptr);
1965       break;
1966    case SPEEX_GET_SUBMODE_ENCODING:
1967       (*(int*)ptr) = st->encode_submode;
1968       break;
1969    case SPEEX_GET_LOOKAHEAD:
1970       (*(int*)ptr)=(st->windowSize-st->frameSize);
1971       break;
1972    case SPEEX_SET_PLC_TUNING:
1973       st->plc_tuning = (*(int*)ptr);
1974       if (st->plc_tuning>100)
1975          st->plc_tuning=100;
1976       break;
1977    case SPEEX_GET_PLC_TUNING:
1978       (*(int*)ptr)=(st->plc_tuning);
1979       break;
1980    case SPEEX_GET_PI_GAIN:
1981       {
1982          int i;
1983          spx_word32_t *g = (spx_word32_t*)ptr;
1984          for (i=0;i<st->nbSubframes;i++)
1985             g[i]=st->pi_gain[i];
1986       }
1987       break;
1988    case SPEEX_GET_EXC:
1989       {
1990          int i;
1991          spx_sig_t *e = (spx_sig_t*)ptr;
1992          for (i=0;i<st->frameSize;i++)
1993             e[i]=st->exc[i];
1994       }
1995       break;
1996    case SPEEX_GET_INNOV:
1997       {
1998          int i;
1999          spx_sig_t *e = (spx_sig_t*)ptr;
2000          for (i=0;i<st->frameSize;i++)
2001             e[i]=st->innov[i];
2002       }
2003       break;
2004    case SPEEX_GET_RELATIVE_QUALITY:
2005       (*(float*)ptr)=st->relative_quality;
2006       break;
2007    default:
2008       speex_warning_int("Unknown nb_ctl request: ", request);
2009       return -1;
2010    }
2011    return 0;
2012 }
2013
2014 int nb_decoder_ctl(void *state, int request, void *ptr)
2015 {
2016    DecState *st;
2017    st=(DecState*)state;
2018    switch(request)
2019    {
2020    case SPEEX_SET_LOW_MODE:
2021    case SPEEX_SET_MODE:
2022       st->submodeID = (*(int*)ptr);
2023       break;
2024    case SPEEX_GET_LOW_MODE:
2025    case SPEEX_GET_MODE:
2026       (*(int*)ptr) = st->submodeID;
2027       break;
2028    case SPEEX_SET_ENH:
2029       st->lpc_enh_enabled = *((int*)ptr);
2030       break;
2031    case SPEEX_GET_ENH:
2032       *((int*)ptr) = st->lpc_enh_enabled;
2033       break;
2034    case SPEEX_GET_FRAME_SIZE:
2035       (*(int*)ptr) = st->frameSize;
2036       break;
2037    case SPEEX_GET_BITRATE:
2038       if (st->submodes[st->submodeID])
2039          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
2040       else
2041          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
2042       break;
2043    case SPEEX_SET_SAMPLING_RATE:
2044       st->sampling_rate = (*(int*)ptr);
2045       break;
2046    case SPEEX_GET_SAMPLING_RATE:
2047       (*(int*)ptr)=st->sampling_rate;
2048       break;
2049    case SPEEX_SET_HANDLER:
2050       {
2051          SpeexCallback *c = (SpeexCallback*)ptr;
2052          st->speex_callbacks[c->callback_id].func=c->func;
2053          st->speex_callbacks[c->callback_id].data=c->data;
2054          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
2055       }
2056       break;
2057    case SPEEX_SET_USER_HANDLER:
2058       {
2059          SpeexCallback *c = (SpeexCallback*)ptr;
2060          st->user_callback.func=c->func;
2061          st->user_callback.data=c->data;
2062          st->user_callback.callback_id=c->callback_id;
2063       }
2064       break;
2065    case SPEEX_RESET_STATE:
2066       {
2067          int i;
2068          for (i=0;i<2*st->lpcSize;i++)
2069             st->mem_sp[i]=0;
2070          for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
2071             st->excBuf[i]=0;
2072          for (i=0;i<st->frameSize;i++)
2073             st->inBuf[i] = 0;
2074       }
2075       break;
2076    case SPEEX_SET_SUBMODE_ENCODING:
2077       st->encode_submode = (*(int*)ptr);
2078       break;
2079    case SPEEX_GET_SUBMODE_ENCODING:
2080       (*(int*)ptr) = st->encode_submode;
2081       break;
2082    case SPEEX_GET_PI_GAIN:
2083       {
2084          int i;
2085          spx_word32_t *g = (spx_word32_t*)ptr;
2086          for (i=0;i<st->nbSubframes;i++)
2087             g[i]=st->pi_gain[i];
2088       }
2089       break;
2090    case SPEEX_GET_EXC:
2091       {
2092          int i;
2093          spx_sig_t *e = (spx_sig_t*)ptr;
2094          for (i=0;i<st->frameSize;i++)
2095             e[i]=st->exc[i];
2096       }
2097       break;
2098    case SPEEX_GET_INNOV:
2099       {
2100          int i;
2101          spx_sig_t *e = (spx_sig_t*)ptr;
2102          for (i=0;i<st->frameSize;i++)
2103             e[i]=st->innov[i];
2104       }
2105       break;
2106    case SPEEX_GET_DTX_STATUS:
2107       *((int*)ptr) = st->dtx_enabled;
2108       break;
2109    default:
2110       speex_warning_int("Unknown nb_ctl request: ", request);
2111       return -1;
2112    }
2113    return 0;
2114 }