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