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