packet loss handling...
[speexdsp.git] / libspeex / nb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: speex.c
3
4    This library is free software; you can redistribute it and/or
5    modify it under the terms of the GNU Lesser General Public
6    License as published by the Free Software Foundation; either
7    version 2.1 of the License, or (at your option) any later version.
8    
9    This library is distributed in the hope that it will be useful,
10    but WITHOUT ANY WARRANTY; without even the implied warranty of
11    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12    Lesser General Public License for more details.
13    
14    You should have received a copy of the GNU Lesser General Public
15    License along with this library; if not, write to the Free Software
16    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17 */
18
19 #include <stdlib.h>
20 #include <stdio.h>
21 #include <math.h>
22 #include "nb_celp.h"
23 #include "lpc.h"
24 #include "lsp.h"
25 #include "ltp.h"
26 #include "quant_lsp.h"
27 #include "cb_search.h"
28 #include "filters.h"
29 #include "stack_alloc.h"
30 #include "vq.h"
31 #include "speex_bits.h"
32 #include "vbr.h"
33 #include "misc.h"
34
35 extern int training_weight;
36 #ifndef M_PI
37 #define M_PI           3.14159265358979323846  /* pi */
38 #endif
39
40 #define SUBMODE(x) st->submodes[st->submodeID]->x
41
42 float exc_gain_quant_scal3[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};
43
44 float exc_gain_quant_scal1[2]={-0.35, 0.05};
45  
46
47 #define sqr(x) ((x)*(x))
48 #define min(a,b) ((a) < (b) ? (a) : (b))
49
50 void *nb_encoder_init(SpeexMode *m)
51 {
52    EncState *st;
53    SpeexNBMode *mode;
54    int i;
55
56    mode=m->mode;
57    st = speex_alloc(sizeof(EncState));
58    st->mode=m;
59    /* Codec parameters, should eventually have several "modes"*/
60    st->frameSize = mode->frameSize;
61    st->windowSize = st->frameSize*3/2;
62    st->nbSubframes=mode->frameSize/mode->subframeSize;
63    st->subframeSize=mode->subframeSize;
64    st->lpcSize = mode->lpcSize;
65    st->bufSize = mode->bufSize;
66    st->gamma1=mode->gamma1;
67    st->gamma2=mode->gamma2;
68    st->min_pitch=mode->pitchStart;
69    st->max_pitch=mode->pitchEnd;
70    st->lag_factor=mode->lag_factor;
71    st->lpc_floor = mode->lpc_floor;
72    st->preemph = mode->preemph;
73   
74    st->submodes=mode->submodes;
75    st->submodeID=mode->defaultSubmode;
76    st->pre_mem=0;
77    st->pre_mem2=0;
78
79    /* Allocating input buffer */
80    st->inBuf = speex_alloc(st->bufSize*sizeof(float));
81    st->frame = st->inBuf + st->bufSize - st->windowSize;
82    /* Allocating excitation buffer */
83    st->excBuf = speex_alloc(st->bufSize*sizeof(float));
84    st->exc = st->excBuf + st->bufSize - st->windowSize;
85    st->swBuf = speex_alloc(st->bufSize*sizeof(float));
86    st->sw = st->swBuf + st->bufSize - st->windowSize;
87
88    st->exc2Buf = speex_alloc(st->bufSize*sizeof(float));
89    st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
90
91    st->innov = speex_alloc(st->frameSize*sizeof(float));
92
93    /* Asymetric "pseudo-Hamming" window */
94    {
95       int part1, part2;
96       part1 = st->subframeSize*7/2;
97       part2 = st->subframeSize*5/2;
98       st->window = speex_alloc(st->windowSize*sizeof(float));
99       for (i=0;i<part1;i++)
100          st->window[i]=.54-.46*cos(M_PI*i/part1);
101       for (i=0;i<part2;i++)
102          st->window[part1+i]=.54+.46*cos(M_PI*i/part2);
103    }
104    /* Create the window for autocorrelation (lag-windowing) */
105    st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(float));
106    for (i=0;i<st->lpcSize+1;i++)
107       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
108
109    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(float));
110
111    st->stack = speex_alloc(20000*sizeof(float));
112
113    st->buf2 = speex_alloc(st->windowSize*sizeof(float));
114
115    st->lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
116    st->interp_lpc = speex_alloc((st->lpcSize+1)*sizeof(float));
117    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
118    st->bw_lpc1 = speex_alloc((st->lpcSize+1)*sizeof(float));
119    st->bw_lpc2 = speex_alloc((st->lpcSize+1)*sizeof(float));
120
121    st->lsp = speex_alloc(st->lpcSize*sizeof(float));
122    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
123    st->old_lsp = speex_alloc(st->lpcSize*sizeof(float));
124    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
125    st->interp_lsp = speex_alloc(st->lpcSize*sizeof(float));
126    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
127    st->rc = speex_alloc(st->lpcSize*sizeof(float));
128    st->first = 1;
129
130    st->mem_sp = speex_alloc(st->lpcSize*sizeof(float));
131    st->mem_sw = speex_alloc(st->lpcSize*sizeof(float));
132
133    st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
134
135    st->pitch = speex_alloc(st->nbSubframes*sizeof(int));
136
137    if (1) {
138       st->vbr = speex_alloc(sizeof(VBRState));
139       vbr_init(st->vbr);
140       st->vbr_quality = 8;
141       st->vbr_enabled = 0;
142    } else {
143       st->vbr = 0;
144    }
145    st->complexity=2;
146
147    return st;
148 }
149
150 void nb_encoder_destroy(void *state)
151 {
152    EncState *st=state;
153    /* Free all allocated memory */
154    speex_free(st->inBuf);
155    speex_free(st->excBuf);
156    speex_free(st->swBuf);
157    speex_free(st->exc2Buf);
158    speex_free(st->innov);
159    speex_free(st->stack);
160
161    speex_free(st->window);
162    speex_free(st->buf2);
163    speex_free(st->lpc);
164    speex_free(st->interp_lpc);
165    speex_free(st->interp_qlpc);
166    
167    speex_free(st->bw_lpc1);
168    speex_free(st->bw_lpc2);
169    speex_free(st->autocorr);
170    speex_free(st->lagWindow);
171    speex_free(st->lsp);
172    speex_free(st->qlsp);
173    speex_free(st->old_lsp);
174    speex_free(st->interp_lsp);
175    speex_free(st->old_qlsp);
176    speex_free(st->interp_qlsp);
177    speex_free(st->rc);
178
179    speex_free(st->mem_sp);
180    speex_free(st->mem_sw);
181    speex_free(st->pi_gain);
182    speex_free(st->pitch);
183
184    vbr_destroy(st->vbr);
185    speex_free(st->vbr);
186
187    /*Free state memory... should be last*/
188    speex_free(st);
189 }
190
191 void nb_encode(void *state, float *in, SpeexBits *bits)
192 {
193    EncState *st;
194    int i, sub, roots;
195    float error;
196    int ol_pitch;
197    float ol_pitch_coef;
198    float ol_gain;
199    float delta_qual=0;
200
201    st=state;
202    
203    /* Copy new data in input buffer */
204    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
205    st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;
206    for (i=1;i<st->frameSize;i++)
207       st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];
208    st->pre_mem = in[st->frameSize-1];
209
210    /* Move signals 1 frame towards the past */
211    speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
212    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
213    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
214
215
216    /* Window for analysis */
217    for (i=0;i<st->windowSize;i++)
218       st->buf2[i] = st->frame[i] * st->window[i];
219
220    /* Compute auto-correlation */
221    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
222
223    st->autocorr[0] += 10;        /* prevents NANs */
224    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
225
226    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
227    for (i=0;i<st->lpcSize+1;i++)
228       st->autocorr[i] *= st->lagWindow[i];
229
230    /* Levinson-Durbin */
231    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
232    st->lpc[0]=1;
233
234    /* LPC to LSPs (x-domain) transform */
235    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 10, 0.01, st->stack);
236    if (roots!=st->lpcSize)
237    {
238       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
239       exit(1);
240    }
241
242    /* LSP x-domain to angle domain*/
243    for (i=0;i<st->lpcSize;i++)
244       st->lsp[i] = acos(st->lsp[i]);
245    /*print_vec(st->lsp, 10, "LSP:");*/
246    /* LSP Quantization */
247    if (st->first)
248    {
249       for (i=0;i<st->lpcSize;i++)
250          st->old_lsp[i] = st->lsp[i];
251    }
252
253
254    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
255    {
256       for (i=0;i<st->lpcSize;i++)
257          st->interp_lsp[i] = .5*st->old_lsp[i] + .5*st->lsp[i];
258
259       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
260
261       /* Compute interpolated LPCs (unquantized) for whole frame*/
262       for (i=0;i<st->lpcSize;i++)
263          st->interp_lsp[i] = cos(st->interp_lsp[i]);
264       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
265
266       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
267       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
268
269       residue(st->frame, st->bw_lpc1, st->exc, st->frameSize, st->lpcSize);
270       syn_filt(st->exc, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize);
271       
272       /*Open-loop pitch*/
273       {
274          int nol_pitch[4];
275          float nol_pitch_coef[4];
276          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
277                                nol_pitch, nol_pitch_coef, 4, st->stack);
278          ol_pitch=nol_pitch[0];
279          ol_pitch_coef = nol_pitch_coef[0];
280          /*Try to remove pitch multiples*/
281          for (i=1;i<4;i++)
282          {
283             if ((nol_pitch_coef[i] > .85*ol_pitch_coef) && 
284                 (fabs(2*nol_pitch[i]-ol_pitch)<=2 || fabs(3*nol_pitch[i]-ol_pitch)<=4 || 
285                  fabs(4*nol_pitch[i]-ol_pitch)<=6 || fabs(5*nol_pitch[i]-ol_pitch)<=8))
286             {
287                /*ol_pitch_coef=nol_pitch_coef[i];*/
288                ol_pitch = nol_pitch[i];
289             }
290          }
291          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
292          /*printf ("ol_pitch: %d %f\n", ol_pitch, ol_pitch_coef);*/
293       }
294       /*Compute "real" excitation*/
295       residue(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize);
296
297       /* Compute open-loop excitation gain */
298       ol_gain=0;
299       for (i=0;i<st->frameSize;i++)
300          ol_gain += st->exc[i]*st->exc[i];
301       
302       ol_gain=sqrt(1+ol_gain/st->frameSize);
303    }
304
305    /*Experimental VBR stuff*/
306    if (st->vbr)
307    {
308       delta_qual = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, ol_pitch_coef);
309       /*if (delta_qual<0)*/
310          delta_qual*=.1*(3+st->vbr_quality);
311       if (st->vbr_enabled) 
312       {
313          int qual = (int)floor(st->vbr_quality+delta_qual+.5);
314          if (qual<1 && delta_qual>-3.5)
315             qual=1;
316          if (qual<0)
317             qual=0;
318          if (qual>10)
319             qual=10;
320          if (qual==10 && st->vbr_quality<10)
321             qual=9;
322          speex_encoder_ctl(state, SPEEX_SET_QUALITY, &qual);
323       }
324    }
325    /*printf ("VBR quality = %f\n", vbr_qual);*/
326
327    /* First, transmit a zero for narrowband */
328    speex_bits_pack(bits, 0, 1);
329
330    /* Transmit the sub-mode we use for this frame */
331    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
332
333
334    /* If null mode (no transmission), just set a couple things to zero*/
335    if (st->submodes[st->submodeID] == NULL)
336    {
337       for (i=0;i<st->frameSize;i++)
338          st->exc[i]=st->exc2[i]=st->sw[i]=0;
339
340       for (i=0;i<st->lpcSize;i++)
341          st->mem_sw[i]=0;
342       st->first=1;
343
344       /* Final signal synthesis from excitation */
345       syn_filt_mem(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
346
347       in[0] = st->frame[0] + st->preemph*st->pre_mem2;
348       for (i=1;i<st->frameSize;i++)
349          in[i]=st->frame[i] + st->preemph*in[i-1];
350       st->pre_mem2=in[st->frameSize-1];
351
352       return;
353
354    }
355
356    /*Quantize LSPs*/
357 #if 1 /*0 for unquantized*/
358    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
359 #else
360    for (i=0;i<st->lpcSize;i++)
361      st->qlsp[i]=st->lsp[i];
362 #endif
363
364    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
365    if (SUBMODE(lbr_pitch)!=-1)
366    {
367       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
368    } 
369    
370    if (SUBMODE(forced_pitch_gain))
371    {
372       int quant;
373       quant = (int)floor(.5+15*ol_pitch_coef);
374       if (quant>15)
375          quant=0;
376       if (quant<0)
377          quant=0;
378       speex_bits_pack(bits, quant, 4);
379       ol_pitch_coef=0.066667*quant;
380    }
381    
382    
383    /*Quantize and transmit open-loop excitation gain*/
384    {
385       int qe = (int)(floor(3.5*log(ol_gain)));
386       if (qe<0)
387          qe=0;
388       if (qe>31)
389          qe=31;
390       ol_gain = exp(qe/3.5);
391       speex_bits_pack(bits, qe, 5);
392    }
393
394    /* Special case for first frame */
395    if (st->first)
396    {
397       for (i=0;i<st->lpcSize;i++)
398          st->old_qlsp[i] = st->qlsp[i];
399    }
400
401    /* Loop on sub-frames */
402    for (sub=0;sub<st->nbSubframes;sub++)
403    {
404       float esig, enoise, snr, tmp;
405       int   offset;
406       float *sp, *sw, *res, *exc, *target, *mem, *exc2;
407       int pitch;
408
409       /* Offset relative to start of frame */
410       offset = st->subframeSize*sub;
411       /* Original signal */
412       sp=st->frame+offset;
413       /* Excitation */
414       exc=st->exc+offset;
415       /* Weighted signal */
416       sw=st->sw+offset;
417
418       exc2=st->exc2+offset;
419
420       /* Filter response */
421       res = PUSH(st->stack, st->subframeSize);
422       /* Target signal */
423       target = PUSH(st->stack, st->subframeSize);
424       mem = PUSH(st->stack, st->lpcSize);
425
426       /* LSP interpolation (quantized and unquantized) */
427       tmp = (1.0 + sub)/st->nbSubframes;
428       for (i=0;i<st->lpcSize;i++)
429          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
430       for (i=0;i<st->lpcSize;i++)
431          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
432
433       /* Make sure the filters are stable */
434       lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);
435       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
436
437       /* Compute interpolated LPCs (quantized and unquantized) */
438       for (i=0;i<st->lpcSize;i++)
439          st->interp_lsp[i] = cos(st->interp_lsp[i]);
440       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
441
442       for (i=0;i<st->lpcSize;i++)
443          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
444       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
445
446       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
447       tmp=1;
448       st->pi_gain[sub]=0;
449       for (i=0;i<=st->lpcSize;i++)
450       {
451          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
452          tmp = -tmp;
453       }
454      
455
456       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
457       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
458       if (st->gamma2>=0)
459          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
460       else
461       {
462          st->bw_lpc2[0]=1;
463          st->bw_lpc2[1]=-st->preemph;
464          for (i=2;i<=st->lpcSize;i++)
465             st->bw_lpc2[i]=0;
466       }
467
468       /* Reset excitation */
469       for (i=0;i<st->subframeSize;i++)
470          exc[i]=0;
471       for (i=0;i<st->subframeSize;i++)
472          exc2[i]=0;
473
474       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
475       for (i=0;i<st->lpcSize;i++)
476          mem[i]=st->mem_sp[i];
477       syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
478       for (i=0;i<st->lpcSize;i++)
479          mem[i]=st->mem_sp[i];
480       residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
481       for (i=0;i<st->lpcSize;i++)
482          mem[i]=st->mem_sw[i];
483       syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
484
485       /* Compute weighted signal */
486       for (i=0;i<st->lpcSize;i++)
487          mem[i]=st->mem_sp[i];
488       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
489       for (i=0;i<st->lpcSize;i++)
490          mem[i]=st->mem_sw[i];
491       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
492       
493       esig=0;
494       for (i=0;i<st->subframeSize;i++)
495          esig+=sw[i]*sw[i];
496       
497       /* Compute target signal */
498       for (i=0;i<st->subframeSize;i++)
499          target[i]=sw[i]-res[i];
500
501       for (i=0;i<st->subframeSize;i++)
502          exc[i]=exc2[i]=0;
503
504       /* If we have a long-term predictor (not all sub-modes have one) */
505       if (SUBMODE(ltp_quant))
506       {
507          int pit_min, pit_max;
508          /* Long-term prediction */
509          if (SUBMODE(lbr_pitch) != -1)
510          {
511             /* Low bit-rate pitch handling */
512             int margin;
513             margin = SUBMODE(lbr_pitch);
514             if (margin)
515             {
516                if (ol_pitch < st->min_pitch+margin-1)
517                   ol_pitch=st->min_pitch+margin-1;
518                if (ol_pitch > st->max_pitch-margin)
519                   ol_pitch=st->max_pitch-margin;
520                pit_min = ol_pitch-margin+1;
521                pit_max = ol_pitch+margin;
522             } else {
523                pit_min=pit_max=ol_pitch;
524             }
525          } else {
526             pit_min = st->min_pitch;
527             pit_max = st->max_pitch;
528          }
529
530          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
531                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
532                                     st->lpcSize, st->subframeSize, bits, st->stack, 
533                                     exc2, st->complexity);
534
535          /*printf ("cl_pitch: %d\n", pitch);*/
536          st->pitch[sub]=pitch;
537       } else {
538          fprintf (stderr, "No pitch prediction, what's wrong\n");
539       }
540
541       /* Update target for adaptive codebook contribution */
542       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
543       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
544       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
545       for (i=0;i<st->subframeSize;i++)
546         target[i]-=res[i];
547
548       /* Compute noise energy and SNR */
549       enoise=0;
550       for (i=0;i<st->subframeSize;i++)
551          enoise += target[i]*target[i];
552       snr = 10*log10((esig+1)/(enoise+1));
553       /*st->pitch[sub]=(int)snr;*/
554 #ifdef DEBUG
555       printf ("pitch SNR = %f\n", snr);
556 #endif
557
558
559       /* Quantization of innovation */
560       {
561          float *innov;
562          float ener=0, ener_1;
563          /*innov=PUSH(st->stack, st->subframeSize);*/
564          innov = st->innov+sub*st->subframeSize;
565          for (i=0;i<st->subframeSize;i++)
566             innov[i]=0;
567          syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
568          residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
569          residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
570          for (i=0;i<st->subframeSize;i++)
571             ener+=st->buf2[i]*st->buf2[i];
572          ener=sqrt(.1+ener/st->subframeSize);
573
574          
575          ener /= ol_gain;
576
577          if (0)
578             printf ("ener: %f %f %f\n", ener, ol_gain, ol_pitch_coef);
579
580          if (SUBMODE(have_subframe_gain)) 
581          {
582             int qe;
583             ener=log(ener);
584             if (SUBMODE(have_subframe_gain)==3)
585             {
586                qe = vq_index(&ener, exc_gain_quant_scal3, 1, 8);
587                speex_bits_pack(bits, qe, 3);
588                ener=exc_gain_quant_scal3[qe];
589             } else {
590                qe = vq_index(&ener, exc_gain_quant_scal1, 1, 2);
591                speex_bits_pack(bits, qe, 1);
592                ener=exc_gain_quant_scal1[qe];               
593             }
594             ener=exp(ener);
595             /*printf ("encode gain: %d %f\n", qe, ener);*/
596          } else {
597             ener=1;
598          }
599
600          ener*=ol_gain;
601          /*printf ("transmit gain: %f\n", ener);*/
602          ener_1 = 1/ener;
603
604          if (0) {
605             int start=rand()%35;
606             printf ("norm_exc: ");
607             for (i=start;i<start+5;i++)
608                printf ("%f ", ener_1*st->buf2[i]);
609             printf ("\n");
610          }
611          
612          for (i=0;i<st->subframeSize;i++)
613             target[i]*=ener_1;
614          
615          if (SUBMODE(innovation_quant))
616          {
617             /* Normal quantization */
618             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
619                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
620                                       innov, bits, st->stack, st->complexity);
621             for (i=0;i<st->subframeSize;i++)
622                innov[i]*=ener;
623             for (i=0;i<st->subframeSize;i++)
624                exc[i] += innov[i];
625          } else {
626             fprintf(stderr, "No fixed codebook\n");
627          }
628
629          if (SUBMODE(double_codebook)) {
630             float *innov2 = PUSH(st->stack, st->subframeSize);
631             for (i=0;i<st->subframeSize;i++)
632                innov2[i]=0;
633             for (i=0;i<st->subframeSize;i++)
634                target[i]*=2.2;
635             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
636                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
637                                       innov2, bits, st->stack, st->complexity);
638             for (i=0;i<st->subframeSize;i++)
639                innov2[i]*=ener*(1/2.2);
640             for (i=0;i<st->subframeSize;i++)
641                exc[i] += innov2[i];
642             POP(st->stack);
643          }
644
645          /*POP(st->stack);*/
646          for (i=0;i<st->subframeSize;i++)
647             target[i]*=ener;
648
649       }
650
651       /* Compute weighted noise energy and SNR */
652       enoise=0;
653       for (i=0;i<st->subframeSize;i++)
654          enoise += target[i]*target[i];
655       snr = 10*log10((esig+1)/(enoise+1));
656 #ifdef DEBUG
657       printf ("seg SNR = %f\n", snr);
658 #endif
659
660       /*Keep the previous memory*/
661       for (i=0;i<st->lpcSize;i++)
662          mem[i]=st->mem_sp[i];
663       /* Final signal synthesis from excitation */
664       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
665
666       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
667       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
668       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
669
670       for (i=0;i<st->subframeSize;i++)
671          exc2[i]=exc[i];
672
673       POP(st->stack);
674       POP(st->stack);
675       POP(st->stack);
676    }
677
678    /* Store the LSPs for interpolation in the next frame */
679    for (i=0;i<st->lpcSize;i++)
680       st->old_lsp[i] = st->lsp[i];
681    for (i=0;i<st->lpcSize;i++)
682       st->old_qlsp[i] = st->qlsp[i];
683
684    /* The next frame will not be the first (Duh!) */
685    st->first = 0;
686
687    /* Replace input by synthesized speech */
688    in[0] = st->frame[0] + st->preemph*st->pre_mem2;
689    for (i=1;i<st->frameSize;i++)
690      in[i]=st->frame[i] + st->preemph*in[i-1];
691    st->pre_mem2=in[st->frameSize-1];
692
693 }
694
695
696 void *nb_decoder_init(SpeexMode *m)
697 {
698    DecState *st;
699    SpeexNBMode *mode;
700    int i;
701
702    mode=m->mode;
703    st = speex_alloc(sizeof(DecState));
704    st->mode=m;
705
706    st->first=1;
707    /* Codec parameters, should eventually have several "modes"*/
708    st->frameSize = mode->frameSize;
709    st->windowSize = st->frameSize*3/2;
710    st->nbSubframes=mode->frameSize/mode->subframeSize;
711    st->subframeSize=mode->subframeSize;
712    st->lpcSize = mode->lpcSize;
713    st->bufSize = mode->bufSize;
714    st->gamma1=mode->gamma1;
715    st->gamma2=mode->gamma2;
716    st->min_pitch=mode->pitchStart;
717    st->max_pitch=mode->pitchEnd;
718    st->preemph = mode->preemph;
719
720    st->submodes=mode->submodes;
721    st->submodeID=mode->defaultSubmode;
722
723    st->pre_mem=0;
724    st->lpc_enh_enabled=0;
725
726    st->stack = speex_alloc(20000*sizeof(float));
727
728    st->inBuf = speex_alloc(st->bufSize*sizeof(float));
729    st->frame = st->inBuf + st->bufSize - st->windowSize;
730    st->excBuf = speex_alloc(st->bufSize*sizeof(float));
731    st->exc = st->excBuf + st->bufSize - st->windowSize;
732    for (i=0;i<st->bufSize;i++)
733       st->inBuf[i]=0;
734    for (i=0;i<st->bufSize;i++)
735       st->excBuf[i]=0;
736    st->innov = speex_alloc(st->frameSize*sizeof(float));
737
738    st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(float));
739    st->qlsp = speex_alloc(st->lpcSize*sizeof(float));
740    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(float));
741    st->interp_qlsp = speex_alloc(st->lpcSize*sizeof(float));
742    st->mem_sp = speex_alloc(5*st->lpcSize*sizeof(float));
743
744    st->pi_gain = speex_alloc(st->nbSubframes*sizeof(float));
745    st->last_pitch = 40;
746    st->count_lost=0;
747    return st;
748 }
749
750 void nb_decoder_destroy(void *state)
751 {
752    DecState *st;
753    st=state;
754    speex_free(st->inBuf);
755    speex_free(st->excBuf);
756    speex_free(st->innov);
757    speex_free(st->interp_qlpc);
758    speex_free(st->qlsp);
759    speex_free(st->old_qlsp);
760    speex_free(st->interp_qlsp);
761    speex_free(st->stack);
762    speex_free(st->mem_sp);
763    speex_free(st->pi_gain);
764    
765    speex_free(state);
766 }
767
768 static void nb_decode_lost(DecState *st, float *out)
769 {
770    int i, sub;
771    /*float exc_ener=0,g;*/
772    /* Shift all buffers by one frame */
773    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
774    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
775
776    for (sub=0;sub<st->nbSubframes;sub++)
777    {
778       int offset;
779       float *sp, *exc;
780       float *num, *den;
781       /* Offset relative to start of frame */
782       offset = st->subframeSize*sub;
783       /* Original signal */
784       sp=st->frame+offset;
785       /* Excitation */
786       exc=st->exc+offset;
787       /* Excitation after post-filter*/
788
789       num=PUSH(st->stack, ((st->lpcSize<<1)+1));
790       den=PUSH(st->stack, ((st->lpcSize<<1)+1));
791       if (st->lpc_enh_enabled)
792       {
793          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
794                  SUBMODE(lpc_enh_k1), SUBMODE(lpc_enh_k2), st->stack);
795       } else {
796          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
797                  SUBMODE(lpc_enh_k2), SUBMODE(lpc_enh_k2), st->stack);
798       }
799         
800       for (i=0;i<st->subframeSize;i++)
801       {
802          exc[i]=st->last_pitch_gain*exc[i-st->last_pitch] + 
803          .8*st->innov[i+offset];
804       }
805
806       for (i=0;i<st->subframeSize;i++)
807          sp[i]=exc[i];
808       
809       pole_zero_mem(sp, num, den, sp, st->subframeSize, (st->lpcSize<<1), 
810                     st->mem_sp+st->lpcSize, st->stack);
811       syn_filt_mem(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
812         st->mem_sp);
813       
814       POP(st->stack);
815       POP(st->stack);
816   
817    }
818
819    out[0] = st->frame[0] + st->preemph*st->pre_mem;
820    for (i=1;i<st->frameSize;i++)
821       out[i]=st->frame[i] + st->preemph*out[i-1];
822    st->pre_mem=out[st->frameSize-1];
823    
824    st->first = 0;
825    st->count_lost++;
826 }
827
828
829 int nb_decode(void *state, SpeexBits *bits, float *out)
830 {
831    DecState *st;
832    int i, sub;
833    int pitch;
834    float pitch_gain[3];
835    float ol_gain;
836    int ol_pitch=0;
837    float ol_pitch_coef=0;
838    int best_pitch=40;
839    float best_pitch_gain=-1;
840    int wideband;
841    int m;
842
843    st=state;
844
845    if (!bits)
846    {
847       nb_decode_lost(st, out);
848       return 0;
849    }
850
851    m = speex_bits_peek_unsigned(bits, 5);
852    if (m==15)
853    {
854       return -1;
855    } else if (m==14)
856    {
857       int req_size = speex_bits_unpack_unsigned(bits, 6);
858       speex_bits_advance(bits, 8*req_size);
859    } else if (m>7)
860    {
861       return -2;
862    }
863
864    wideband = speex_bits_unpack_unsigned(bits, 1);
865    if (wideband)
866    {
867       int submode;
868       int advance;
869       submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
870       advance = submode;
871       speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
872       advance -= (SB_SUBMODE_BITS+1);
873       speex_bits_advance(bits, advance);
874       wideband = speex_bits_unpack_unsigned(bits, 1);
875       if (wideband)
876       {
877          fprintf (stderr, "Corrupted stream\n");
878       }
879    }
880
881    /* Get the sub-mode that was used */
882    st->submodeID = speex_bits_unpack_unsigned(bits, NB_SUBMODE_BITS);
883
884    /* Shift all buffers by one frame */
885    speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
886    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
887
888    /* If null mode (no transmission), just set a couple things to zero*/
889    if (st->submodes[st->submodeID] == NULL)
890    {
891       for (i=0;i<st->frameSize;i++)
892          st->exc[i]=0;
893       st->first=1;
894       
895       /* Final signal synthesis from excitation */
896       syn_filt_mem(st->exc, st->interp_qlpc, st->frame, st->subframeSize, st->lpcSize, st->mem_sp);
897
898       out[0] = st->frame[0] + st->preemph*st->pre_mem;
899       for (i=1;i<st->frameSize;i++)
900          out[i]=st->frame[i] + st->preemph*out[i-1];
901       st->pre_mem=out[st->frameSize-1];
902       st->count_lost=0;
903       return 0;
904    }
905
906    /* Unquantize LSPs */
907    SUBMODE(lsp_unquant)(st->qlsp, st->lpcSize, bits);
908
909    /* Handle first frame and lost-packet case */
910    if (st->first || st->count_lost)
911    {
912       for (i=0;i<st->lpcSize;i++)
913          st->old_qlsp[i] = st->qlsp[i];
914    }
915
916    /* Get open-loop pitch estimation for low bit-rate pitch coding */
917    if (SUBMODE(lbr_pitch)!=-1)
918    {
919       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
920    } 
921    
922    if (SUBMODE(forced_pitch_gain))
923    {
924       int quant;
925       quant = speex_bits_unpack_unsigned(bits, 4);
926       ol_pitch_coef=0.066667*quant;
927       /*fprintf (stderr, "unquant pitch coef: %f\n", ol_pitch_coef);*/
928    }
929    
930    /* Get global excitation gain */
931    {
932       int qe;
933       qe = speex_bits_unpack_unsigned(bits, 5);
934       ol_gain = exp(qe/3.5);
935       /*printf ("decode_ol_gain: %f\n", ol_gain);*/
936    }
937
938    /*Loop on subframes */
939    for (sub=0;sub<st->nbSubframes;sub++)
940    {
941       int offset;
942       float *sp, *exc, tmp;
943       float *num, *den;
944       /* Offset relative to start of frame */
945       offset = st->subframeSize*sub;
946       /* Original signal */
947       sp=st->frame+offset;
948       /* Excitation */
949       exc=st->exc+offset;
950       /* Excitation after post-filter*/
951
952       /* LSP interpolation (quantized and unquantized) */
953       tmp = (1.0 + sub)/st->nbSubframes;
954       for (i=0;i<st->lpcSize;i++)
955          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
956
957       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, .002);
958
959
960       /* Compute interpolated LPCs (unquantized) */
961       for (i=0;i<st->lpcSize;i++)
962          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
963       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
964
965       num=PUSH(st->stack, ((st->lpcSize<<1)+1));
966       den=PUSH(st->stack, ((st->lpcSize<<1)+1));
967       if (st->lpc_enh_enabled)
968       {
969          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
970                  SUBMODE(lpc_enh_k1), SUBMODE(lpc_enh_k2), st->stack);
971       } else {
972          enh_lpc(st->interp_qlpc, st->lpcSize, num, den, 
973                  SUBMODE(lpc_enh_k2), SUBMODE(lpc_enh_k2), st->stack);
974       }
975       /* Compute analysis filter at w=pi */
976       tmp=1;
977       st->pi_gain[sub]=0;
978       for (i=0;i<=st->lpcSize;i++)
979       {
980          st->pi_gain[sub] += tmp*st->interp_qlpc[i];
981          tmp = -tmp;
982       }
983
984       /* Reset excitation */
985       for (i=0;i<st->subframeSize;i++)
986          exc[i]=0;
987
988       /*Adaptive codebook contribution*/
989       if (SUBMODE(ltp_unquant))
990       {
991          int pit_min, pit_max;
992          if (SUBMODE(lbr_pitch) != -1)
993          {
994             int margin;
995             margin = SUBMODE(lbr_pitch);
996             if (margin)
997             {
998                if (ol_pitch < st->min_pitch+margin-1)
999                   ol_pitch=st->min_pitch+margin-1;
1000                if (ol_pitch > st->max_pitch-margin)
1001                   ol_pitch=st->max_pitch-margin;
1002                pit_min = ol_pitch-margin+1;
1003                pit_max = ol_pitch+margin;
1004             } else {
1005                pit_min=pit_max=ol_pitch;
1006             }
1007          } else {
1008             pit_min = st->min_pitch;
1009             pit_max = st->max_pitch;
1010          }
1011
1012          SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1013                               st->subframeSize, &pitch, &pitch_gain[0], bits, st->stack, st->count_lost);
1014          
1015          tmp = (pitch_gain[0]+pitch_gain[1]+pitch_gain[2]);
1016          if (tmp>best_pitch_gain)
1017          {
1018             best_pitch = pitch;
1019             /*while (best_pitch+pitch<st->max_pitch)
1020             {
1021                best_pitch+=pitch;
1022                }*/
1023             best_pitch_gain = tmp*.9;
1024             if (best_pitch_gain>.85)
1025                best_pitch_gain=.85;
1026          }
1027       } else {
1028          fprintf (stderr, "No pitch prediction, what's wrong\n");
1029       }
1030       
1031       /* Unquantize the innovation */
1032       {
1033          int q_energy;
1034          float ener;
1035          float *innov;
1036          
1037          /*innov = PUSH(st->stack, st->subframeSize);*/
1038          innov = st->innov+sub*st->subframeSize;
1039          for (i=0;i<st->subframeSize;i++)
1040             innov[i]=0;
1041
1042          if (SUBMODE(have_subframe_gain)==3)
1043          {
1044             q_energy = speex_bits_unpack_unsigned(bits, 3);
1045             ener = ol_gain*exp(exc_gain_quant_scal3[q_energy]);
1046          } else if (SUBMODE(have_subframe_gain)==1)
1047          {
1048             q_energy = speex_bits_unpack_unsigned(bits, 1);
1049             ener = ol_gain*exp(exc_gain_quant_scal1[q_energy]);
1050          } else {
1051             ener = ol_gain;
1052          }
1053          
1054          /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
1055          
1056          if (SUBMODE(innovation_unquant))
1057          {
1058             /*Fixed codebook contribution*/
1059             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, st->stack);
1060          } else {
1061             fprintf(stderr, "No fixed codebook\n");
1062          }
1063
1064          for (i=0;i<st->subframeSize;i++)
1065             innov[i]*=ener;
1066          for (i=0;i<st->subframeSize;i++)
1067             exc[i]+=innov[i];
1068
1069          if (SUBMODE(double_codebook))
1070          {
1071             float *innov2 = PUSH(st->stack, st->subframeSize);
1072             for (i=0;i<st->subframeSize;i++)
1073                innov2[i]=0;
1074             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, st->stack);
1075             for (i=0;i<st->subframeSize;i++)
1076                innov2[i]*=ener*(1/2.2);
1077             for (i=0;i<st->subframeSize;i++)
1078                exc[i] += innov2[i];
1079             POP(st->stack);
1080          }
1081
1082          /*POP(st->stack);*/
1083       }
1084
1085       for (i=0;i<st->subframeSize;i++)
1086          sp[i]=exc[i];
1087
1088       if (st->lpc_enh_enabled && SUBMODE(comb_gain>0))
1089          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1090                               pitch, pitch_gain, .5);
1091       pole_zero_mem(sp, num, den, sp, st->subframeSize, (st->lpcSize<<1), 
1092                     st->mem_sp+st->lpcSize, st->stack);
1093       syn_filt_mem(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1094         st->mem_sp);
1095       
1096       POP(st->stack);
1097       POP(st->stack);
1098    }
1099    
1100    /*Copy output signal*/
1101    out[0] = st->frame[0] + st->preemph*st->pre_mem;
1102    for (i=1;i<st->frameSize;i++)
1103      out[i]=st->frame[i] + st->preemph*out[i-1];
1104    st->pre_mem=out[st->frameSize-1];
1105
1106
1107    /* Store the LSPs for interpolation in the next frame */
1108    for (i=0;i<st->lpcSize;i++)
1109       st->old_qlsp[i] = st->qlsp[i];
1110
1111    /* The next frame will not be the first (Duh!) */
1112    st->first = 0;
1113    st->count_lost=0;
1114    st->last_pitch = best_pitch;
1115    st->last_pitch_gain = best_pitch_gain;
1116
1117    return 0;
1118 }
1119
1120 void nb_encoder_ctl(void *state, int request, void *ptr)
1121 {
1122    EncState *st;
1123    st=state;     
1124    switch(request)
1125    {
1126    case SPEEX_GET_FRAME_SIZE:
1127       (*(int*)ptr) = st->frameSize;
1128       break;
1129    case SPEEX_SET_MODE:
1130       st->submodeID = (*(int*)ptr);
1131       break;
1132    case SPEEX_GET_MODE:
1133       (*(int*)ptr) = st->submodeID;
1134       break;
1135    case SPEEX_SET_VBR:
1136       st->vbr_enabled = (*(int*)ptr);
1137       break;
1138    case SPEEX_GET_VBR:
1139       (*(int*)ptr) = st->vbr_enabled;
1140       break;
1141    case SPEEX_SET_VBR_QUALITY:
1142       st->vbr_quality = (*(int*)ptr);
1143       break;
1144    case SPEEX_GET_VBR_QUALITY:
1145       (*(int*)ptr) = st->vbr_quality;
1146       break;
1147    case SPEEX_SET_QUALITY:
1148       {
1149          int quality = (*(int*)ptr);
1150          if (quality<=0)
1151             st->submodeID = 0;
1152          else if (quality<=1)
1153             st->submodeID = 1;
1154          else if (quality<=2)
1155             st->submodeID = 2;
1156          else if (quality<=4)
1157             st->submodeID = 3;
1158          else if (quality<=6)
1159             st->submodeID = 4;
1160          else if (quality<=8)
1161             st->submodeID = 5;
1162          else if (quality<=9)
1163             st->submodeID = 6;
1164          else if (quality<=10)
1165             st->submodeID = 7;
1166          else
1167             fprintf(stderr, "Unknown nb_ctl quality: %d\n", quality);
1168       }
1169       break;
1170    case SPEEX_SET_COMPLEXITY:
1171       st->complexity = (*(int*)ptr);
1172       break;
1173    case SPEEX_GET_COMPLEXITY:
1174       (*(int*)ptr) = st->complexity;
1175       break;
1176    case SPEEX_GET_BITRATE:
1177       if (st->submodes[st->submodeID])
1178          (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
1179       else
1180          (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
1181       break;
1182    default:
1183       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1184    }
1185 }
1186
1187 void nb_decoder_ctl(void *state, int request, void *ptr)
1188 {
1189    DecState *st;
1190    st=state;
1191    switch(request)
1192    {
1193    case SPEEX_SET_ENH:
1194       st->lpc_enh_enabled = *((int*)ptr);
1195       break;
1196    case SPEEX_GET_ENH:
1197       *((int*)ptr) = st->lpc_enh_enabled;
1198       break;
1199    case SPEEX_GET_FRAME_SIZE:
1200       (*(int*)ptr) = st->frameSize;
1201       break;
1202    case SPEEX_GET_BITRATE:
1203       if (st->submodes[st->submodeID])
1204          (*(int*)ptr) = 50*SUBMODE(bits_per_frame);
1205       else
1206          (*(int*)ptr) = 50*(NB_SUBMODE_BITS+1);
1207       break;
1208    default:
1209       fprintf(stderr, "Unknown nb_ctl request: %d\n", request);
1210    }
1211 }