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