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