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