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