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