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