Think DTX now works for wideband too
[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 #include <stdlib.h>
33 #include <stdio.h>
34 #include <math.h>
35 #include "nb_celp.h"
36 #include "lpc.h"
37 #include "lsp.h"
38 #include "ltp.h"
39 #include "quant_lsp.h"
40 #include "cb_search.h"
41 #include "filters.h"
42 #include "stack_alloc.h"
43 #include "vq.h"
44 #include "speex_bits.h"
45 #include "vbr.h"
46 #include "misc.h"
47 #include "speex_callbacks.h"
48
49 #ifdef SLOW_TRIG
50 #include "math_approx.h"
51 #define cos speex_cos
52 #endif
53
54 extern int training_weight;
55 #ifndef M_PI
56 #define M_PI           3.14159265358979323846  /* pi */
57 #endif
58
59 #define SUBMODE(x) st->submodes[st->submodeID]->x
60
61 float exc_gain_quant_scal3[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
62
63 float exc_gain_quant_scal1[2]={-0.35, 0.05};
64
65 #define sqr(x) ((x)*(x))
66
67 #ifndef min
68 #define min(a,b) ((a) < (b) ? (a) : (b))
69 #endif
70
71 void *nb_encoder_init(SpeexMode *m)
72 {
73    EncState *st;
74    SpeexNBMode *mode;
75    int i;
76
77    mode=(SpeexNBMode *)m->mode;
78    st = (EncState*)speex_alloc(sizeof(EncState));
79    st->mode=m;
80
81    st->frameSize = mode->frameSize;
82    st->windowSize = st->frameSize*3/2;
83    st->nbSubframes=mode->frameSize/mode->subframeSize;
84    st->subframeSize=mode->subframeSize;
85    st->lpcSize = mode->lpcSize;
86    st->bufSize = mode->bufSize;
87    st->gamma1=mode->gamma1;
88    st->gamma2=mode->gamma2;
89    st->min_pitch=mode->pitchStart;
90    st->max_pitch=mode->pitchEnd;
91    st->lag_factor=mode->lag_factor;
92    st->lpc_floor = mode->lpc_floor;
93    st->preemph = mode->preemph;
94   
95    st->submodes=mode->submodes;
96    st->submodeID=st->submodeSelect=mode->defaultSubmode;
97    st->pre_mem=0;
98    st->pre_mem2=0;
99    st->bounded_pitch = 1;
100
101    /* Allocating input buffer */
102    st->inBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
103    st->frame = st->inBuf + st->bufSize - st->windowSize;
104    /* Allocating excitation buffer */
105    st->excBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
106    st->exc = st->excBuf + st->bufSize - st->windowSize;
107    st->swBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
108    st->sw = st->swBuf + st->bufSize - st->windowSize;
109
110    st->exc2Buf = (float*)speex_alloc(st->bufSize*sizeof(float));
111    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
112
113    st->innov = (float*)speex_alloc(st->frameSize*sizeof(float));
114
115    /* Asymetric "pseudo-Hamming" window */
116    {
117       int part1, part2;
118       part1 = st->subframeSize*7/2;
119       part2 = st->subframeSize*5/2;
120       st->window = (float*)speex_alloc(st->windowSize*sizeof(float));
121       for (i=0;i<part1;i++)
122          st->window[i]=.54-.46*cos(M_PI*i/part1);
123       for (i=0;i<part2;i++)
124          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
125    }
126    /* Create the window for autocorrelation (lag-windowing) */
127    st->lagWindow = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
128    for (i=0;i<st->lpcSize+1;i++)
129       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
130
131    st->autocorr = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
132
133    st->stack = (float*)speex_alloc(4000*sizeof(float));
134
135    st->buf2 = (float*)speex_alloc(st->windowSize*sizeof(float));
136
137    st->lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
138    st->interp_lpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
139    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
140    st->bw_lpc1 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
141    st->bw_lpc2 = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
142
143    st->lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
144    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
145    st->old_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
146    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
147    st->interp_lsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
148    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
149    st->rc = (float*)speex_alloc(st->lpcSize*sizeof(float));
150    st->first = 1;
151    for (i=0;i<st->lpcSize;i++)
152    {
153       st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
154    }
155
156    st->mem_sp = (float*)speex_alloc(st->lpcSize*sizeof(float));
157    st->mem_sw = (float*)speex_alloc(st->lpcSize*sizeof(float));
158    st->mem_sw_whole = (float*)speex_alloc(st->lpcSize*sizeof(float));
159    st->mem_exc = (float*)speex_alloc(st->lpcSize*sizeof(float));
160
161    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
162
163    st->pitch = (int*)speex_alloc(st->nbSubframes*sizeof(int));
164
165    st->vbr = (VBRState*)speex_alloc(sizeof(VBRState));
166    vbr_init(st->vbr);
167    st->vbr_quality = 8;
168    st->vbr_enabled = 0;
169    st->vad_enabled = 0;
170    st->dtx_enabled = 0;
171    st->abr_enabled = 0;
172    st->abr_drift = 0;
173
174    st->complexity=2;
175    st->sampling_rate=8000;
176
177    return st;
178 }
179
180 void nb_encoder_destroy(void *state)
181 {
182    EncState *st=(EncState *)state;
183    /* Free all allocated memory */
184    speex_free(st->inBuf);
185    speex_free(st->excBuf);
186    speex_free(st->swBuf);
187    speex_free(st->exc2Buf);
188    speex_free(st->innov);
189    speex_free((float*)st->stack);
190
191    speex_free(st->window);
192    speex_free(st->buf2);
193    speex_free(st->lpc);
194    speex_free(st->interp_lpc);
195    speex_free(st->interp_qlpc);
196    
197    speex_free(st->bw_lpc1);
198    speex_free(st->bw_lpc2);
199    speex_free(st->autocorr);
200    speex_free(st->lagWindow);
201    speex_free(st->lsp);
202    speex_free(st->qlsp);
203    speex_free(st->old_lsp);
204    speex_free(st->interp_lsp);
205    speex_free(st->old_qlsp);
206    speex_free(st->interp_qlsp);
207    speex_free(st->rc);
208
209    speex_free(st->mem_sp);
210    speex_free(st->mem_sw);
211    speex_free(st->mem_sw_whole);
212    speex_free(st->mem_exc);
213    speex_free(st->pi_gain);
214    speex_free(st->pitch);
215
216    vbr_destroy(st->vbr);
217    speex_free(st->vbr);
218
219    /*Free state memory... should be last*/
220    speex_free((float*)st);
221 }
222
223 void nb_encode(void *state, float *in, SpeexBits *bits)
224 {
225    EncState *st;
226    int i, sub, roots;
227    int ol_pitch;
228    float ol_pitch_coef;
229    float ol_gain;
230    float *res, *target, *mem;
231    void *stack;
232    float *syn_resp;
233    float lsp_dist=0;
234    float *orig;
235
236    st=(EncState *)state;
237    stack=st->stack;
238
239    /* Copy new data in input buffer */
240    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
241    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
242    for (i=1;i<st->frameSize;i++)
243       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
244    st->pre_mem = in[st->frameSize-1];
245
246    /* Move signals 1 frame towards the past */
247    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
248    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
249    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
250
251
252    /* Window for analysis */
253    for (i=0;i<st->windowSize;i++)
254       st->buf2[i] = st->frame[i] * st->window[i];
255
256    /* Compute auto-correlation */
257    _spx_autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
258
259    st->autocorr[0] += 10;        /* prevents NANs */
260    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
261
262    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
263    for (i=0;i<st->lpcSize+1;i++)
264       st->autocorr[i] *= st->lagWindow[i];
265
266    /* Levinson-Durbin */
267    wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
268    st->lpc[0]=1;
269
270    /* LPC to LSPs (x-domain) transform */
271    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);
272    /* Check if we found all the roots */
273    if (roots==st->lpcSize)
274    {
275       /* LSP x-domain to angle domain*/
276       for (i=0;i<st->lpcSize;i++)
277          st->lsp[i] = acos(st->lsp[i]);
278    } else {
279       /* Search again if we can afford it */
280       if (st->complexity>1)
281          roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.05, stack);
282       if (roots==st->lpcSize) 
283       {
284          /* LSP x-domain to angle domain*/
285          for (i=0;i<st->lpcSize;i++)
286             st->lsp[i] = acos(st->lsp[i]);
287       } else {
288          /*If we can't find all LSP's, do some damage control and use previous filter*/
289          for (i=0;i<st->lpcSize;i++)
290          {
291             st->lsp[i]=st->old_lsp[i];
292          }
293       }
294    }
295
296
297    lsp_dist=0;
298    for (i=0;i<st->lpcSize;i++)
299       lsp_dist += (st->old_lsp[i] - st->lsp[i])*(st->old_lsp[i] - st->lsp[i]);
300    /*printf ("%f\n", lsp_dist);*/
301
302    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
303    {
304       if (st->first)
305          for (i=0;i<st->lpcSize;i++)
306             st->interp_lsp[i] = st->lsp[i];
307       else
308          for (i=0;i<st->lpcSize;i++)
309             st->interp_lsp[i] = .375*st->old_lsp[i] + .625*st->lsp[i];
310
311       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
312
313       /* Compute interpolated LPCs (unquantized) for whole frame*/
314       for (i=0;i<st->lpcSize;i++)
315          st->interp_lsp[i] = cos(st->interp_lsp[i]);
316       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
317
318
319       /*Open-loop pitch*/
320       if (!st->submodes[st->submodeID] || st->vbr_enabled || st->vad_enabled || SUBMODE(forced_pitch_gain) ||
321           SUBMODE(lbr_pitch) != -1)
322       {
323          int nol_pitch[6];
324          float nol_pitch_coef[6];
325          
326          bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
327          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
328          
329          filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);
330
331          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
332                                nol_pitch, nol_pitch_coef, 6, stack);
333          ol_pitch=nol_pitch[0];
334          ol_pitch_coef = nol_pitch_coef[0];
335          /*printf ("%f %d %d %d %d %d %d ", ol_pitch_coef, nol_pitch[0], nol_pitch[1], nol_pitch[2], nol_pitch[3], nol_pitch[4], nol_pitch[5]);*/
336          /*Try to remove pitch multiples*/
337          for (i=1;i<6;i++)
338          {
339             if ((nol_pitch_coef[i]>.85*ol_pitch_coef) && 
340                 (fabs(nol_pitch[i]-ol_pitch/2.0)<=1 || fabs(nol_pitch[i]-ol_pitch/3.0)<=1 || 
341                  fabs(nol_pitch[i]-ol_pitch/4.0)<=1 || fabs(nol_pitch[i]-ol_pitch/5.0)<=1))
342             {
343                /*ol_pitch_coef=nol_pitch_coef[i];*/
344                ol_pitch = nol_pitch[i];
345             }
346          }
347          /*printf ("%d\n", ol_pitch);*/
348          /*if (ol_pitch>50)
349            ol_pitch/=2;*/
350          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
351          /*printf ("ol_pitch: %d %f\n", ol_pitch, ol_pitch_coef);*/
352       } else {
353          ol_pitch=0;
354          ol_pitch_coef=0;
355       }
356       /*Compute "real" excitation*/
357       fir_mem2(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc);
358
359       /* Compute open-loop excitation gain */
360       ol_gain=0;
361       for (i=0;i<st->frameSize;i++)
362          ol_gain += st->exc[i]*st->exc[i];
363       /*for (i=0;i<160;i++)
364         printf ("%f\n", st->exc[i]);*/
365       
366       ol_gain=sqrt(1+ol_gain/st->frameSize);
367    }
368
369    /*VBR stuff*/
370    if (st->vbr && (st->vbr_enabled||st->vad_enabled))
371    {
372       
373       if (st->abr_enabled)
374       {
375          float qual_change=0;
376          if (st->abr_drift2 * st->abr_drift > 0)
377          {
378             /* Only adapt if long-term and short-term drift are the same sign */
379             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
380             if (qual_change>.05)
381                qual_change=.05;
382             if (qual_change<-.05)
383                qual_change=-.05;
384          }
385          st->vbr_quality += qual_change;
386          if (st->vbr_quality>10)
387             st->vbr_quality=10;
388          if (st->vbr_quality<0)
389             st->vbr_quality=0;
390          printf ("%f %f\n", st->abr_drift/(.1+st->abr_count), st->vbr_quality);
391       }
392
393       st->relative_quality = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
394       /*if (delta_qual<0)*/
395       /*  delta_qual*=.1*(3+st->vbr_quality);*/
396       if (st->vbr_enabled) 
397       {
398          int mode;
399          mode = 7;
400          while (mode)
401          {
402             int v1;
403             float thresh;
404             v1=(int)floor(st->vbr_quality);
405             if (v1==10)
406                thresh = vbr_nb_thresh[mode][v1];
407             else
408                thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
409             if (st->relative_quality > thresh)
410                break;
411             mode--;
412          }
413          /*fprintf(stderr, "");
414          fprintf (stderr, "encode %f %d\n", st->relative_quality, mode);
415          fprintf(stderr, "encode: %d %d\n",st->submodeID, mode);*/
416
417          speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
418
419          if (st->abr_enabled)
420          {
421             int bitrate;
422             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
423             st->abr_drift+=(bitrate-st->abr_enabled);
424             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
425             st->abr_count += 1.0;
426          }
427          /*fprintf(stderr, "encode: %d %d\n",st->submodeID, mode);*/
428       } else {
429          /*VAD only case*/
430          int mode;
431          if (st->relative_quality<4)
432          {
433             if (st->submodeID>1 || lsp_dist>.05 || !st->dtx_enabled)
434                mode=1;
435             else
436             {
437                mode=0;
438                fprintf (stderr, "tata\n");
439             }
440          } else
441             mode=st->submodeSelect;
442          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
443          st->submodeID=mode;
444       } 
445    } else {
446       st->relative_quality = -1;
447    }
448    /*printf ("VBR quality = %f\n", vbr_qual);*/
449
450    /* First, transmit a zero for narrowband */
451    speex_bits_pack(bits, 0, 1);
452
453    /* Transmit the sub-mode we use for this frame */
454    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
455
456
457    /* If null mode (no transmission), just set a couple things to zero*/
458    if (st->submodes[st->submodeID] == NULL)
459    {
460       for (i=0;i<st->frameSize;i++)
461          st->exc[i]=st->exc2[i]=st->sw[i]=0;
462
463       for (i=0;i<st->lpcSize;i++)
464          st->mem_sw[i]=0;
465       st->first=1;
466       st->bounded_pitch = 1;
467
468       /* Final signal synthesis from excitation */
469       iir_mem2(st->exc, st->interp_qlpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
470
471       in[0] = st->frame[0] + st->preemph*st->pre_mem2;
472       for (i=1;i<st->frameSize;i++)
473          in[i]=st->frame[i] + st->preemph*in[i-1];
474       st->pre_mem2=in[st->frameSize-1];
475
476       return;
477
478    }
479
480    /* LSP Quantization */
481    if (st->first)
482    {
483       for (i=0;i<st->lpcSize;i++)
484          st->old_lsp[i] = st->lsp[i];
485    }
486
487
488    /*Quantize LSPs*/
489 #if 1 /*0 for unquantized*/
490    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
491 #else
492    for (i=0;i<st->lpcSize;i++)
493      st->qlsp[i]=st->lsp[i];
494 #endif
495
496    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
497    if (SUBMODE(lbr_pitch)!=-1)
498    {
499       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
500    } 
501    
502    if (SUBMODE(forced_pitch_gain))
503    {
504       int quant;
505       quant = (int)floor(.5+15*ol_pitch_coef);
506       if (quant>15)
507          quant=0;
508       if (quant<0)
509          quant=0;
510       speex_bits_pack(bits, quant, 4);
511       ol_pitch_coef=0.066667*quant;
512    }
513    
514    
515    /*Quantize and transmit open-loop excitation gain*/
516    {
517       int qe = (int)(floor(3.5*log(ol_gain)));
518       if (qe<0)
519          qe=0;
520       if (qe>31)
521          qe=31;
522       ol_gain = exp(qe/3.5);
523       speex_bits_pack(bits, qe, 5);
524    }
525
526    /* Special case for first frame */
527    if (st->first)
528    {
529       for (i=0;i<st->lpcSize;i++)
530          st->old_qlsp[i] = st->qlsp[i];
531    }
532
533    /* Filter response */
534    res = PUSH(stack, st->subframeSize, float);
535    /* Target signal */
536    target = PUSH(stack, st->subframeSize, float);
537    syn_resp = PUSH(stack, st->subframeSize, float);
538    mem = PUSH(stack, st->lpcSize, float);
539    orig = PUSH(stack, st->frameSize, float);
540    for (i=0;i<st->frameSize;i++)
541       orig[i]=st->frame[i];
542
543    /* Loop on sub-frames */
544    for (sub=0;sub<st->nbSubframes;sub++)
545    {
546       float tmp;
547       int   offset;
548       float *sp, *sw, *exc, *exc2;
549       int pitch;
550
551       /* Offset relative to start of frame */
552       offset = st->subframeSize*sub;
553       /* Original signal */
554       sp=st->frame+offset;
555       /* Excitation */
556       exc=st->exc+offset;
557       /* Weighted signal */
558       sw=st->sw+offset;
559
560       exc2=st->exc2+offset;
561
562
563       /* LSP interpolation (quantized and unquantized) */
564       tmp = (1.0 + sub)/st->nbSubframes;
565       for (i=0;i<st->lpcSize;i++)
566          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
567       for (i=0;i<st->lpcSize;i++)
568          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
569
570       /* Make sure the filters are stable */
571       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
572       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
573
574       /* Compute interpolated LPCs (quantized and unquantized) */
575       for (i=0;i<st->lpcSize;i++)
576          st->interp_lsp[i] = cos(st->interp_lsp[i]);
577       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
578
579       for (i=0;i<st->lpcSize;i++)
580          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
581       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
582
583       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
584       tmp=1;
585       st->pi_gain[sub]=0;
586       for (i=0;i<=st->lpcSize;i++)
587       {
588          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
589          tmp = -tmp;
590       }
591
592       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
593       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
594       if (st->gamma2>=0)
595          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
596       else
597       {
598          st->bw_lpc2[0]=1;
599          st->bw_lpc2[1]=-st->preemph;
600          for (i=2;i<=st->lpcSize;i++)
601             st->bw_lpc2[i]=0;
602       }
603
604       /* Compute impulse response of A(z/g1) / ( A(z)*A(z/g2) )*/
605       for (i=0;i<st->subframeSize;i++)
606          exc[i]=0;
607       exc[0]=1;
608       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, st->subframeSize, st->lpcSize, stack);
609
610       /* Reset excitation */
611       for (i=0;i<st->subframeSize;i++)
612          exc[i]=0;
613       for (i=0;i<st->subframeSize;i++)
614          exc2[i]=0;
615
616       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
617       for (i=0;i<st->lpcSize;i++)
618          mem[i]=st->mem_sp[i];
619       iir_mem2(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
620       
621       for (i=0;i<st->lpcSize;i++)
622          mem[i]=st->mem_sw[i];
623       filter_mem2(exc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
624       
625       /* Compute weighted signal */
626       for (i=0;i<st->lpcSize;i++)
627          mem[i]=st->mem_sw[i];
628       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
629       
630       /* Compute target signal */
631       for (i=0;i<st->subframeSize;i++)
632          target[i]=sw[i]-res[i];
633
634       for (i=0;i<st->subframeSize;i++)
635          exc[i]=exc2[i]=0;
636
637       /* If we have a long-term predictor (otherwise, something's wrong) */
638       if (SUBMODE(ltp_quant))
639       {
640          int pit_min, pit_max;
641          /* Long-term prediction */
642          if (SUBMODE(lbr_pitch) != -1)
643          {
644             /* Low bit-rate pitch handling */
645             int margin;
646             margin = SUBMODE(lbr_pitch);
647             if (margin)
648             {
649                if (ol_pitch < st->min_pitch+margin-1)
650                   ol_pitch=st->min_pitch+margin-1;
651                if (ol_pitch > st->max_pitch-margin)
652                   ol_pitch=st->max_pitch-margin;
653                pit_min = ol_pitch-margin+1;
654                pit_max = ol_pitch+margin;
655             } else {
656                pit_min=pit_max=ol_pitch;
657             }
658          } else {
659             pit_min = st->min_pitch;
660             pit_max = st->max_pitch;
661          }
662          
663          /* Force pitch to use only the current frame if needed */
664          if (st->bounded_pitch && pit_max>offset)
665             pit_max=offset;
666
667          /* Perform pitch search */
668          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
669                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
670                                     st->lpcSize, st->subframeSize, bits, stack, 
671                                     exc2, syn_resp, st->complexity);
672
673          st->pitch[sub]=pitch;
674       } else {
675          fprintf (stderr, "No pitch prediction, what's wrong\n");
676       }
677
678       /* Update target for adaptive codebook contribution */
679       syn_percep_zero(exc, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, res, st->subframeSize, st->lpcSize, stack);
680       for (i=0;i<st->subframeSize;i++)
681          target[i]-=res[i];
682
683
684       /* Quantization of innovation */
685       {
686          float *innov;
687          float ener=0, ener_1;
688
689          innov = st->innov+sub*st->subframeSize;
690          for (i=0;i<st->subframeSize;i++)
691             innov[i]=0;
692          
693          residue_percep_zero(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize, stack);
694          for (i=0;i<st->subframeSize;i++)
695             ener+=st->buf2[i]*st->buf2[i];
696          ener=sqrt(.1+ener/st->subframeSize);
697          /*for (i=0;i<st->subframeSize;i++)
698             printf ("%f\n", st->buf2[i]/ener);
699          */
700          
701          ener /= ol_gain;
702
703          /* Calculate gain correction for the sub-frame (if any) */
704          if (SUBMODE(have_subframe_gain)) 
705          {
706             int qe;
707             ener=log(ener);
708             if (SUBMODE(have_subframe_gain)==3)
709             {
710                qe = vq_index(&ener, exc_gain_quant_scal3, 1, 8);
711                speex_bits_pack(bits, qe, 3);
712                ener=exc_gain_quant_scal3[qe];
713             } else {
714                qe = vq_index(&ener, exc_gain_quant_scal1, 1, 2);
715                speex_bits_pack(bits, qe, 1);
716                ener=exc_gain_quant_scal1[qe];               
717             }
718             ener=exp(ener);
719          } else {
720             ener=1;
721          }
722
723          ener*=ol_gain;
724
725          /*printf ("%f %f\n", ener, ol_gain);*/
726
727          ener_1 = 1/ener;
728
729          if (0) {
730             int start=rand()%35;
731             printf ("norm_exc: ");
732             for (i=start;i<start+5;i++)
733                printf ("%f ", ener_1*st->buf2[i]);
734             printf ("\n");
735          }
736          
737          /* Normalize innovation */
738          for (i=0;i<st->subframeSize;i++)
739             target[i]*=ener_1;
740          
741          /* Quantize innovation */
742          if (SUBMODE(innovation_quant))
743          {
744             /* Codebook search */
745             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
746                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
747                                       innov, syn_resp, bits, stack, st->complexity);
748             
749             /* De-normalize innovation and update excitation */
750             for (i=0;i<st->subframeSize;i++)
751                innov[i]*=ener;
752             for (i=0;i<st->subframeSize;i++)
753                exc[i] += innov[i];
754          } else {
755             fprintf(stderr, "No fixed codebook\n");
756          }
757
758          /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
759          if (SUBMODE(double_codebook)) {
760             void *tmp_stack=stack;
761             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
762             for (i=0;i<st->subframeSize;i++)
763                innov2[i]=0;
764             for (i=0;i<st->subframeSize;i++)
765                target[i]*=2.2;
766             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
767                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
768                                       innov2, syn_resp, bits, tmp_stack, st->complexity);
769             for (i=0;i<st->subframeSize;i++)
770                innov2[i]*=ener*(1/2.2);
771             for (i=0;i<st->subframeSize;i++)
772                exc[i] += innov2[i];
773          }
774
775          for (i=0;i<st->subframeSize;i++)
776             target[i]*=ener;
777
778       }
779
780       /*Keep the previous memory*/
781       for (i=0;i<st->lpcSize;i++)
782          mem[i]=st->mem_sp[i];
783       /* Final signal synthesis from excitation */
784       iir_mem2(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
785
786       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
787       filter_mem2(sp, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
788       for (i=0;i<st->subframeSize;i++)
789          exc2[i]=exc[i];
790    }
791
792    /* Store the LSPs for interpolation in the next frame */
793    if (st->submodeID>=1)
794    {
795       for (i=0;i<st->lpcSize;i++)
796          st->old_lsp[i] = st->lsp[i];
797       for (i=0;i<st->lpcSize;i++)
798          st->old_qlsp[i] = st->qlsp[i];
799    }
800
801    /* The next frame will not be the first (Duh!) */
802    st->first = 0;
803
804    {
805       float ener=0, err=0;
806       float snr;
807       for (i=0;i<st->frameSize;i++)
808       {
809          ener+=st->frame[i]*st->frame[i];
810          err += (st->frame[i]-orig[i])*(st->frame[i]-orig[i]);
811       }
812       snr = 10*log10((ener+1)/(err+1));
813       /*printf ("%f %f %f\n", snr, ener, err);*/
814    }
815
816    /* Replace input by synthesized speech */
817    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
818    for (i=1;i<st->frameSize;i++)
819      in[i]=st->frame[i] + st->preemph*in[i-1];
820    st->pre_mem2=in[st->frameSize-1];
821
822    if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
823       st->bounded_pitch = 1;
824    else
825       st->bounded_pitch = 0;
826 }
827
828
829 void *nb_decoder_init(SpeexMode *m)
830 {
831    DecState *st;
832    SpeexNBMode *mode;
833    int i;
834
835    mode=(SpeexNBMode*)m->mode;
836    st = (DecState *)speex_alloc(sizeof(DecState));
837    st->mode=m;
838
839    st->first=1;
840    /* Codec parameters, should eventually have several "modes"*/
841    st->frameSize = mode->frameSize;
842    st->windowSize = st->frameSize*3/2;
843    st->nbSubframes=mode->frameSize/mode->subframeSize;
844    st->subframeSize=mode->subframeSize;
845    st->lpcSize = mode->lpcSize;
846    st->bufSize = mode->bufSize;
847    st->gamma1=mode->gamma1;
848    st->gamma2=mode->gamma2;
849    st->min_pitch=mode->pitchStart;
850    st->max_pitch=mode->pitchEnd;
851    st->preemph = mode->preemph;
852
853    st->submodes=mode->submodes;
854    st->submodeID=mode->defaultSubmode;
855
856    st->pre_mem=0;
857    st->lpc_enh_enabled=0;
858
859    st->stack = speex_alloc(2000*sizeof(float));
860
861    st->inBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
862    st->frame = st->inBuf + st->bufSize - st->windowSize;
863    st->excBuf = (float*)speex_alloc(st->bufSize*sizeof(float));
864    st->exc = st->excBuf + st->bufSize - st->windowSize;
865    for (i=0;i<st->bufSize;i++)
866       st->inBuf[i]=0;
867    for (i=0;i<st->bufSize;i++)
868       st->excBuf[i]=0;
869    st->innov = (float*)speex_alloc(st->frameSize*sizeof(float));
870
871    st->interp_qlpc = (float*)speex_alloc((st->lpcSize+1)*sizeof(float));
872    st->qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
873    st->old_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
874    st->interp_qlsp = (float*)speex_alloc(st->lpcSize*sizeof(float));
875    st->mem_sp = (float*)speex_alloc(5*st->lpcSize*sizeof(float));
876
877    st->pi_gain = (float*)speex_alloc(st->nbSubframes*sizeof(float));
878    st->last_pitch = 40;
879    st->count_lost=0;
880    st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
881    st->pitch_gain_buf_idx = 0;
882
883    st->sampling_rate=8000;
884    st->last_ol_gain = 0;
885
886    st->user_callback.func = &speex_default_user_handler;
887    st->user_callback.data = NULL;
888    for (i=0;i<16;i++)
889       st->speex_callbacks[i].func = NULL;
890
891    st->voc_m1=st->voc_m2=st->voc_mean=0;
892    st->voc_offset=0;
893
894    return st;
895 }
896
897 void nb_decoder_destroy(void *state)
898 {
899    DecState *st;
900    st=(DecState*)state;
901    speex_free(st->inBuf);
902    speex_free(st->excBuf);
903    speex_free(st->innov);
904    speex_free(st->interp_qlpc);
905    speex_free(st->qlsp);
906    speex_free(st->old_qlsp);
907    speex_free(st->interp_qlsp);
908    speex_free(st->stack);
909    speex_free(st->mem_sp);
910    speex_free(st->pi_gain);
911    
912    speex_free(state);
913 }
914
915 #define median3(a, b, c)        ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
916
917 static void nb_decode_lost(DecState *st, float *out, void *stack)
918 {
919    int i, sub;
920    float *awk1, *awk2, *awk3;
921    float pitch_gain, fact, gain_med;
922
923    fact = exp(-.04*st->count_lost*st->count_lost);
924    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
925    if (gain_med < st->last_pitch_gain)
926       st->last_pitch_gain = gain_med;
927    
928    pitch_gain = st->last_pitch_gain;
929    if (pitch_gain>.95)
930       pitch_gain=.95;
931
932    pitch_gain *= fact;
933    
934 #ifdef DEBUG
935    printf ("recovered unquantized pitch: %d, gain: %f (fact=%f, buf=[%f %f %f], med=%f)\n", st->last_pitch, pitch_gain, fact, st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2], gain_med);
936 #endif
937
938    /* Shift all buffers by one frame */
939    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
940    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
941
942    awk1=PUSH(stack, (st->lpcSize+1), float);
943    awk2=PUSH(stack, (st->lpcSize+1), float);
944    awk3=PUSH(stack, (st->lpcSize+1), float);
945
946    for (sub=0;sub<st->nbSubframes;sub++)
947    {
948       int offset;
949       float *sp, *exc;
950       /* Offset relative to start of frame */
951       offset = st->subframeSize*sub;
952       /* Original signal */
953       sp=st->frame+offset;
954       /* Excitation */
955       exc=st->exc+offset;
956       /* Excitation after post-filter*/
957
958       /* Calculate perceptually enhanced LPC filter */
959       if (st->lpc_enh_enabled)
960       {
961          float r=.9;
962          
963          float k1,k2,k3;
964          k1=SUBMODE(lpc_enh_k1);
965          k2=SUBMODE(lpc_enh_k2);
966          k3=(1-(1-r*k1)/(1-r*k2))/r;
967          if (!st->lpc_enh_enabled)
968          {
969             k1=k2;
970             k3=0;
971          }
972          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
973          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
974          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
975       }
976         
977       /* Make up a plausible excitation */
978       /* THIS CAN BE IMPROVED */
979       /*if (pitch_gain>.95)
980         pitch_gain=.95;*/
981       /*printf ("%f\n", pitch_gain);*/
982       {
983          float innov_gain=0;
984          for (i=0;i<st->frameSize;i++)
985             innov_gain += st->innov[i]*st->innov[i];
986          innov_gain=sqrt(innov_gain/160);
987       for (i=0;i<st->subframeSize;i++)
988       {
989 #if 0
990          exc[i] = pitch_gain * exc[i - st->last_pitch] + fact*sqrt(1-pitch_gain)*st->innov[i+offset];
991          /*Just so it give the same lost packets as with if 0*/
992          rand();
993 #else
994          /*exc[i]=pitch_gain*exc[i-st->last_pitch] +  fact*st->innov[i+offset];*/
995          exc[i]=pitch_gain*exc[i-st->last_pitch] + 
996          fact*sqrt(1-pitch_gain)*3*innov_gain*((((float)rand())/RAND_MAX)-.5);
997 #endif
998       }
999       }
1000       for (i=0;i<st->subframeSize;i++)
1001          sp[i]=exc[i];
1002       
1003       /* Signal synthesis */
1004       if (st->lpc_enh_enabled)
1005       {
1006          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1007                      st->mem_sp+st->lpcSize);
1008          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1009                      st->mem_sp);
1010       } else {
1011          for (i=0;i<st->lpcSize;i++)
1012             st->mem_sp[st->lpcSize+i] = 0;
1013          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1014                      st->mem_sp);
1015       }      
1016    }
1017
1018    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1019    for (i=1;i<st->frameSize;i++)
1020       out[i]=st->frame[i] + st->preemph*out[i-1];
1021    st->pre_mem=out[st->frameSize-1];
1022    
1023    st->first = 0;
1024    st->count_lost++;
1025    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = pitch_gain;
1026    if (st->pitch_gain_buf_idx > 2) /* rollover */
1027       st->pitch_gain_buf_idx = 0;
1028 }
1029
1030 int nb_decode(void *state, SpeexBits *bits, float *out)
1031 {
1032    DecState *st;
1033    int i, sub;
1034    int pitch;
1035    float pitch_gain[3];
1036    float ol_gain=0;
1037    int ol_pitch=0;
1038    float ol_pitch_coef=0;
1039    int best_pitch=40;
1040    float best_pitch_gain=0;
1041    int wideband;
1042    int m;
1043    void *stack;
1044    float *awk1, *awk2, *awk3;
1045    float pitch_average=0;
1046
1047    st=(DecState*)state;
1048    stack=st->stack;
1049
1050    /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1051    if (!bits)
1052    {
1053       nb_decode_lost(st, out, stack);
1054       return 0;
1055    }
1056
1057    /* Search for next narrwoband block (handle requests, skip wideband blocks) */
1058    do {
1059       wideband = speex_bits_unpack_unsigned(bits, 1);
1060       if (wideband) /* Skip wideband block (for compatibility) */
1061       {
1062          int submode;
1063          int advance;
1064          advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1065          speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1066          advance -= (SB_SUBMODE_BITS+1);
1067          speex_bits_advance(bits, advance);
1068          wideband = speex_bits_unpack_unsigned(bits, 1);
1069          if (wideband)
1070          {
1071             fprintf (stderr, "Corrupted stream?\n");
1072          }
1073       }
1074
1075       m = speex_bits_unpack_unsigned(bits, 4);
1076       if (m==15) /* We found a terminator */
1077       {
1078          return -1;
1079       } else if (m==14) /* Speex in-band request */
1080       {
1081          int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1082          if (ret)
1083             return ret;
1084       } else if (m==13) /* User in-band request */
1085       {
1086          int ret = st->user_callback.func(bits, state, st->user_callback.data);
1087          if (ret)
1088             return ret;
1089       } else if (m>7) /* Invalid mode */
1090       {
1091          return -2;
1092       }
1093       
1094    } while (m>7);
1095
1096    /* Get the sub-mode that was used */
1097    st->submodeID = m;
1098
1099    /* Shift all buffers by one frame */
1100    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1101    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
1102
1103    /* If null mode (no transmission), just set a couple things to zero*/
1104    if (st->submodes[st->submodeID] == NULL)
1105    {
1106       float *lpc;
1107       lpc = PUSH(stack,11, float);
1108       bw_lpc(.93, st->interp_qlpc, lpc, 10);
1109       /*for (i=0;i<st->frameSize;i++)
1110         st->exc[i]=0;*/
1111       {
1112          float innov_gain=0;
1113          float pitch_gain=st->last_pitch_gain;
1114          if (pitch_gain>.6)
1115             pitch_gain=.6;
1116          for (i=0;i<st->frameSize;i++)
1117             innov_gain += st->innov[i]*st->innov[i];
1118          innov_gain=sqrt(innov_gain/160);
1119          for (i=0;i<st->frameSize;i++)
1120          {
1121             st->exc[i]=3*innov_gain*((((float)rand())/RAND_MAX)-.5);
1122          }
1123       }
1124
1125
1126       st->first=1;
1127
1128       /* Final signal synthesis from excitation */
1129       iir_mem2(st->exc, lpc, st->frame, st->frameSize, st->lpcSize, st->mem_sp);
1130
1131       out[0] = st->frame[0] + st->preemph*st->pre_mem;
1132       for (i=1;i<st->frameSize;i++)
1133          out[i]=st->frame[i] + st->preemph*out[i-1];
1134       st->pre_mem=out[st->frameSize-1];
1135       st->count_lost=0;
1136       return 0;
1137    }
1138
1139    /* Unquantize LSPs */
1140    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
1141
1142    /*Damp memory if a frame was lost and the LSP changed too much*/
1143    if (st->count_lost)
1144    {
1145       float lsp_dist=0, fact;
1146       for (i=0;i<st->lpcSize;i++)
1147          lsp_dist += fabs(st->old_qlsp[i] - st->qlsp[i]);
1148       fact = .6*exp(-.2*lsp_dist);
1149       for (i=0;i<2*st->lpcSize;i++)
1150          st->mem_sp[i] *= fact;
1151    }
1152
1153
1154    /* Handle first frame and lost-packet case */
1155    if (st->first || st->count_lost)
1156    {
1157       for (i=0;i<st->lpcSize;i++)
1158          st->old_qlsp[i] = st->qlsp[i];
1159    }
1160
1161    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1162    if (SUBMODE(lbr_pitch)!=-1)
1163    {
1164       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1165    } 
1166    
1167    if (SUBMODE(forced_pitch_gain))
1168    {
1169       int quant;
1170       quant = speex_bits_unpack_unsigned(bits, 4);
1171       ol_pitch_coef=0.066667*quant;
1172       /*fprintf (stderr, "unquant pitch coef: %f\n", ol_pitch_coef);*/
1173    }
1174    
1175    /* Get global excitation gain */
1176    {
1177       int qe;
1178       qe = speex_bits_unpack_unsigned(bits, 5);
1179       ol_gain = exp(qe/3.5);
1180       /*printf ("decode_ol_gain: %f\n", ol_gain);*/
1181    }
1182
1183    awk1=PUSH(stack, st->lpcSize+1, float);
1184    awk2=PUSH(stack, st->lpcSize+1, float);
1185    awk3=PUSH(stack, st->lpcSize+1, float);
1186
1187    /*Loop on subframes */
1188    for (sub=0;sub<st->nbSubframes;sub++)
1189    {
1190       int offset;
1191       float *sp, *exc, tmp;
1192
1193       /* Offset relative to start of frame */
1194       offset = st->subframeSize*sub;
1195       /* Original signal */
1196       sp=st->frame+offset;
1197       /* Excitation */
1198       exc=st->exc+offset;
1199       /* Excitation after post-filter*/
1200
1201       /* LSP interpolation (quantized and unquantized) */
1202       tmp = (1.0 + sub)/st->nbSubframes;
1203       for (i=0;i<st->lpcSize;i++)
1204          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
1205
1206       /* Make sure the LSP's are stable */
1207       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
1208
1209
1210       /* Compute interpolated LPCs (unquantized) */
1211       for (i=0;i<st->lpcSize;i++)
1212          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
1213       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
1214
1215       /* Compute enhanced synthesis filter */
1216       if (st->lpc_enh_enabled)
1217       {
1218          float r=.9;
1219          
1220          float k1,k2,k3;
1221          k1=SUBMODE(lpc_enh_k1);
1222          k2=SUBMODE(lpc_enh_k2);
1223          k3=(1-(1-r*k1)/(1-r*k2))/r;
1224          k3=k1-k2;
1225          if (!st->lpc_enh_enabled)
1226          {
1227             k1=k2;
1228             k3=0;
1229          }
1230          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1231          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1232          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1233          
1234       }
1235
1236       /* Compute analysis filter at w=pi */
1237       tmp=1;
1238       st->pi_gain[sub]=0;
1239       for (i=0;i<=st->lpcSize;i++)
1240       {
1241          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
1242          tmp = -tmp;
1243       }
1244
1245       /* Reset excitation */
1246       for (i=0;i<st->subframeSize;i++)
1247          exc[i]=0;
1248
1249       /*Adaptive codebook contribution*/
1250       if (SUBMODE(ltp_unquant))
1251       {
1252          int pit_min, pit_max;
1253          /* Handle pitch constraints if any */
1254          if (SUBMODE(lbr_pitch) != -1)
1255          {
1256             int margin;
1257             margin = SUBMODE(lbr_pitch);
1258             if (margin)
1259             {
1260 /* GT - need optimization?
1261                if (ol_pitch < st->min_pitch+margin-1)
1262                   ol_pitch=st->min_pitch+margin-1;
1263                if (ol_pitch > st->max_pitch-margin)
1264                   ol_pitch=st->max_pitch-margin;
1265                pit_min = ol_pitch-margin+1;
1266                pit_max = ol_pitch+margin;
1267 */
1268                pit_min = ol_pitch-margin+1;
1269                if (pit_min < st->min_pitch)
1270                   pit_min = st->min_pitch;
1271                pit_max = ol_pitch+margin;
1272                if (pit_max > st->max_pitch)
1273                   pit_max = st->max_pitch;
1274             } else {
1275                pit_min = pit_max = ol_pitch;
1276             }
1277          } else {
1278             pit_min = st->min_pitch;
1279             pit_max = st->max_pitch;
1280          }
1281
1282          /* Pitch synthesis */
1283          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1284                               st->subframeSize, &pitch, &pitch_gain[0], bits, stack, st->count_lost, offset, st->last_pitch_gain);
1285          
1286          /* If we had lost frames, check energy of last received frame */
1287          if (st->count_lost && ol_gain < st->last_ol_gain)
1288          {
1289             float fact = ol_gain/(st->last_ol_gain+1);
1290             for (i=0;i<st->subframeSize;i++)
1291                exc[i]*=fact;
1292          }
1293
1294          tmp = fabs(pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1295          tmp = fabs(pitch_gain[1]);
1296          if (pitch_gain[0]>0)
1297             tmp += pitch_gain[0];
1298          else
1299             tmp -= .5*pitch_gain[0];
1300          if (pitch_gain[2]>0)
1301             tmp += pitch_gain[2];
1302          else
1303             tmp -= .5*pitch_gain[0];
1304
1305
1306          pitch_average += tmp;
1307          if (tmp>best_pitch_gain)
1308          {
1309             best_pitch = pitch;
1310             best_pitch_gain = tmp;
1311             /*            best_pitch_gain = tmp*.9;
1312                         if (best_pitch_gain>.85)
1313                         best_pitch_gain=.85;*/
1314          }
1315       } else {
1316          fprintf (stderr, "No pitch prediction, what's wrong\n");
1317       }
1318       
1319       /* Unquantize the innovation */
1320       {
1321          int q_energy;
1322          float ener;
1323          float *innov;
1324          
1325          innov = st->innov+sub*st->subframeSize;
1326          for (i=0;i<st->subframeSize;i++)
1327             innov[i]=0;
1328
1329          /* Decode sub-frame gain correction */
1330          if (SUBMODE(have_subframe_gain)==3)
1331          {
1332             q_energy = speex_bits_unpack_unsigned(bits, 3);
1333             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1334          } else if (SUBMODE(have_subframe_gain)==1)
1335          {
1336             q_energy = speex_bits_unpack_unsigned(bits, 1);
1337             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1338          } else {
1339             ener = ol_gain;
1340          }
1341          
1342          /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
1343          
1344          if (SUBMODE(innovation_unquant))
1345          {
1346             /*Fixed codebook contribution*/
1347             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1348          } else {
1349             fprintf(stderr, "No fixed codebook\n");
1350          }
1351
1352          /* De-normalize innovation and update excitation */
1353          for (i=0;i<st->subframeSize;i++)
1354             innov[i]*=ener;
1355
1356          /*Vocoder mode*/
1357          if (st->submodeID==1) 
1358          {
1359             float g=ol_pitch_coef;
1360
1361             
1362             for (i=0;i<st->subframeSize;i++)
1363                exc[i]=0;
1364             while (st->voc_offset<st->subframeSize)
1365             {
1366                if (st->voc_offset>=0)
1367                   exc[st->voc_offset]=sqrt(1.0*ol_pitch);
1368                st->voc_offset+=ol_pitch;
1369             }
1370             st->voc_offset -= st->subframeSize;
1371
1372             g=.5+2*(g-.6);
1373             if (g<0)
1374                g=0;
1375             if (g>1)
1376                g=1;
1377             for (i=0;i<st->subframeSize;i++)
1378             {
1379                int tmp=exc[i];
1380                exc[i]=.8*g*exc[i]*ol_gain + .6*g*st->voc_m1*ol_gain + .5*g*innov[i] - .5*g*st->voc_m2 + (1-g)*innov[i];
1381                st->voc_m1 = tmp;
1382                st->voc_m2=innov[i];
1383                st->voc_mean = .95*st->voc_mean + .05*exc[i];
1384                exc[i]-=st->voc_mean;
1385             }
1386          } else {
1387             for (i=0;i<st->subframeSize;i++)
1388                exc[i]+=innov[i];
1389          }
1390          /*printf ("%f %f\n", ener, ol_gain);*/
1391          /* Decode second codebook (only for some modes) */
1392          if (SUBMODE(double_codebook))
1393          {
1394             void *tmp_stack=stack;
1395             float *innov2 = PUSH(tmp_stack, st->subframeSize, float);
1396             for (i=0;i<st->subframeSize;i++)
1397                innov2[i]=0;
1398             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, tmp_stack);
1399             for (i=0;i<st->subframeSize;i++)
1400                innov2[i]*=ener*(1/2.2);
1401             for (i=0;i<st->subframeSize;i++)
1402                exc[i] += innov2[i];
1403          }
1404
1405       }
1406
1407       for (i=0;i<st->subframeSize;i++)
1408          sp[i]=exc[i];
1409
1410       /* Signal synthesis */
1411       if (st->lpc_enh_enabled && SUBMODE(comb_gain>0))
1412          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1413                               pitch, pitch_gain, .5);
1414       if (st->lpc_enh_enabled)
1415       {
1416          /* Use enhanced LPC filter */
1417          filter_mem2(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1418                      st->mem_sp+st->lpcSize);
1419          filter_mem2(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1420                      st->mem_sp);
1421       } else {
1422          /* Use regular filter */
1423          for (i=0;i<st->lpcSize;i++)
1424             st->mem_sp[st->lpcSize+i] = 0;
1425          iir_mem2(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1426                      st->mem_sp);
1427       }
1428    }
1429    
1430    /*Copy output signal*/
1431    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1432    for (i=1;i<st->frameSize;i++)
1433      out[i]=st->frame[i] + st->preemph*out[i-1];
1434    st->pre_mem=out[st->frameSize-1];
1435
1436
1437    /* Store the LSPs for interpolation in the next frame */
1438    for (i=0;i<st->lpcSize;i++)
1439       st->old_qlsp[i] = st->qlsp[i];
1440
1441    /* The next frame will not be the first (Duh!) */
1442    st->first = 0;
1443    st->count_lost=0;
1444    st->last_pitch = best_pitch;
1445    st->last_pitch_gain = .25*pitch_average;
1446    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1447    if (st->pitch_gain_buf_idx > 2) /* rollover */
1448       st->pitch_gain_buf_idx = 0;
1449
1450    st->last_ol_gain = ol_gain;
1451
1452
1453 #ifdef DEBUG
1454 printf ("last pitch: {%d, %f} buf=[%f %f %f]\n", st->last_pitch, st->last_pitch_gain, st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
1455 #endif
1456
1457    return 0;
1458 }
1459
1460 void nb_encoder_ctl(void *state, int request, void *ptr)
1461 {
1462    EncState *st;
1463    st=(EncState*)state;     
1464    switch(request)
1465    {
1466    case SPEEX_GET_FRAME_SIZE:
1467       (*(int*)ptr) = st->frameSize;
1468       break;
1469    case SPEEX_SET_LOW_MODE:
1470    case SPEEX_SET_MODE:
1471       st->submodeSelect = st->submodeID = (*(int*)ptr);
1472       break;
1473    case SPEEX_GET_LOW_MODE:
1474    case SPEEX_GET_MODE:
1475       (*(int*)ptr) = st->submodeID;
1476       break;
1477    case SPEEX_SET_VBR:
1478       st->vbr_enabled = (*(int*)ptr);
1479       break;
1480    case SPEEX_GET_VBR:
1481       (*(int*)ptr) = st->vbr_enabled;
1482       break;
1483    case SPEEX_SET_VAD:
1484       st->vad_enabled = (*(int*)ptr);
1485       break;
1486    case SPEEX_GET_VAD:
1487       (*(int*)ptr) = st->vad_enabled;
1488       break;
1489    case SPEEX_SET_DTX:
1490       st->dtx_enabled = (*(int*)ptr);
1491       break;
1492    case SPEEX_GET_DTX:
1493       (*(int*)ptr) = st->dtx_enabled;
1494       break;
1495    case SPEEX_SET_ABR:
1496       st->abr_enabled = (*(int*)ptr);
1497       st->vbr_enabled = 1;
1498       {
1499          int i=10, rate, target;
1500          float vbr_qual;
1501          target = (*(int*)ptr);
1502          while (i>=0)
1503          {
1504             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1505             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1506             if (rate <= target)
1507                break;
1508             i--;
1509          }
1510          vbr_qual=i;
1511          if (vbr_qual<0)
1512             vbr_qual=0;
1513          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1514          st->abr_count=0;
1515          st->abr_drift=0;
1516          st->abr_drift2=0;
1517       }
1518       
1519       break;
1520    case SPEEX_GET_ABR:
1521       (*(int*)ptr) = st->abr_enabled;
1522       break;
1523    case SPEEX_SET_VBR_QUALITY:
1524       st->vbr_quality = (*(float*)ptr);
1525       break;
1526    case SPEEX_GET_VBR_QUALITY:
1527       (*(float*)ptr) = st->vbr_quality;
1528       break;
1529    case SPEEX_SET_QUALITY:
1530       {
1531          int quality = (*(int*)ptr);
1532          if (quality < 0)
1533             quality = 0;
1534          if (quality > 10)
1535             quality = 10;
1536          st->submodeSelect = st->submodeID = ((SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1537       }
1538       break;
1539    case SPEEX_SET_COMPLEXITY:
1540       st->complexity = (*(int*)ptr);
1541       if (st->complexity<1)
1542          st->complexity=1;
1543       break;
1544    case SPEEX_GET_COMPLEXITY:
1545       (*(int*)ptr) = st->complexity;
1546       break;
1547    case SPEEX_SET_BITRATE:
1548       {
1549          int i=10, rate, target;
1550          target = (*(int*)ptr);
1551          while (i>=0)
1552          {
1553             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1554             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1555             if (rate <= target)
1556                break;
1557             i--;
1558          }
1559       }
1560       break;
1561    case SPEEX_GET_BITRATE:
1562       if (st->submodes[st->submodeID])
1563          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1564       else
1565          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1566       break;
1567    case SPEEX_SET_SAMPLING_RATE:
1568       st->sampling_rate = (*(int*)ptr);
1569       break;
1570    case SPEEX_GET_SAMPLING_RATE:
1571       (*(int*)ptr)=st->sampling_rate;
1572       break;
1573    case SPEEX_RESET_STATE:
1574       {
1575          int i;
1576          st->bounded_pitch = 1;
1577          st->first = 1;
1578          for (i=0;i<st->lpcSize;i++)
1579             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1580          for (i=0;i<st->lpcSize;i++)
1581             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1582          for (i=0;i<st->bufSize;i++)
1583             st->excBuf[i]=st->swBuf[i]=st->inBuf[i]=st->exc2Buf[i]=0;
1584       }
1585       break;
1586    case SPEEX_GET_PI_GAIN:
1587       {
1588          int i;
1589          float *g = (float*)ptr;
1590          for (i=0;i<st->nbSubframes;i++)
1591             g[i]=st->pi_gain[i];
1592       }
1593       break;
1594    case SPEEX_GET_EXC:
1595       {
1596          int i;
1597          float *e = (float*)ptr;
1598          for (i=0;i<st->frameSize;i++)
1599             e[i]=st->exc[i];
1600       }
1601       break;
1602    case SPEEX_GET_INNOV:
1603       {
1604          int i;
1605          float *e = (float*)ptr;
1606          for (i=0;i<st->frameSize;i++)
1607             e[i]=st->innov[i];
1608       }
1609       break;
1610    case SPEEX_GET_RELATIVE_QUALITY:
1611       (*(float*)ptr)=st->relative_quality;
1612       break;
1613    default:
1614       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1615    }
1616 }
1617
1618 void nb_decoder_ctl(void *state, int request, void *ptr)
1619 {
1620    DecState *st;
1621    st=(DecState*)state;
1622    switch(request)
1623    {
1624    case SPEEX_GET_LOW_MODE:
1625    case SPEEX_GET_MODE:
1626       (*(int*)ptr) = st->submodeID;
1627       break;
1628    case SPEEX_SET_ENH:
1629       st->lpc_enh_enabled = *((int*)ptr);
1630       break;
1631    case SPEEX_GET_ENH:
1632       *((int*)ptr) = st->lpc_enh_enabled;
1633       break;
1634    case SPEEX_GET_FRAME_SIZE:
1635       (*(int*)ptr) = st->frameSize;
1636       break;
1637    case SPEEX_GET_BITRATE:
1638       if (st->submodes[st->submodeID])
1639          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1640       else
1641          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1642       break;
1643    case SPEEX_SET_SAMPLING_RATE:
1644       st->sampling_rate = (*(int*)ptr);
1645       break;
1646    case SPEEX_GET_SAMPLING_RATE:
1647       (*(int*)ptr)=st->sampling_rate;
1648       break;
1649    case SPEEX_SET_HANDLER:
1650       {
1651          SpeexCallback *c = (SpeexCallback*)ptr;
1652          st->speex_callbacks[c->callback_id].func=c->func;
1653          st->speex_callbacks[c->callback_id].data=c->data;
1654          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
1655       }
1656       break;
1657    case SPEEX_SET_USER_HANDLER:
1658       {
1659          SpeexCallback *c = (SpeexCallback*)ptr;
1660          st->user_callback.func=c->func;
1661          st->user_callback.data=c->data;
1662          st->user_callback.callback_id=c->callback_id;
1663       }
1664       break;
1665    case SPEEX_RESET_STATE:
1666       {
1667          int i;
1668          for (i=0;i<2*st->lpcSize;i++)
1669             st->mem_sp[i]=0;
1670          for (i=0;i<st->bufSize;i++)
1671             st->excBuf[i]=st->inBuf[i]=0;
1672       }
1673       break;
1674    case SPEEX_GET_PI_GAIN:
1675       {
1676          int i;
1677          float *g = (float*)ptr;
1678          for (i=0;i<st->nbSubframes;i++)
1679             g[i]=st->pi_gain[i];
1680       }
1681       break;
1682    case SPEEX_GET_EXC:
1683       {
1684          int i;
1685          float *e = (float*)ptr;
1686          for (i=0;i<st->frameSize;i++)
1687             e[i]=st->exc[i];
1688       }
1689       break;
1690    case SPEEX_GET_INNOV:
1691       {
1692          int i;
1693          float *e = (float*)ptr;
1694          for (i=0;i<st->frameSize;i++)
1695             e[i]=st->innov[i];
1696       }
1697       break;
1698    default:
1699       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1700    }
1701 }