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