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