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