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