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