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