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