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