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