Wideband almost done but buggy...
[speexdsp.git] / libspeex / speex.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 "speex.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
32 extern float stoc[];
33 extern float exc_table[][8];
34 #ifndef M_PI
35 #define M_PI           3.14159265358979323846  /* pi */
36 #endif
37
38 #define sqr(x) ((x)*(x))
39 #define min(a,b) ((a) < (b) ? (a) : (b))
40
41 void encoder_init(EncState *st, SpeexMode *mode)
42 {
43    int i;
44    float tmp;
45    /* Codec parameters, should eventually have several "modes"*/
46    st->frameSize = mode->frameSize;
47    st->windowSize = mode->windowSize;
48    st->nbSubframes=mode->frameSize/mode->subframeSize;
49    st->subframeSize=mode->subframeSize;
50    st->lpcSize = mode->lpcSize;
51    st->bufSize = mode->bufSize;
52    st->gamma1=mode->gamma1;
53    st->gamma2=mode->gamma2;
54    st->min_pitch=mode->pitchStart;
55    st->max_pitch=mode->pitchEnd;
56    st->lag_factor=mode->lag_factor;
57    st->lpc_floor = mode->lpc_floor;
58
59    st->lsp_quant = mode->lsp_quant;
60    st->ltp_quant = mode->ltp_quant;
61    st->ltp_params = mode->ltp_params;
62    st->innovation_quant = mode->innovation_quant;
63    st->innovation_params = mode->innovation_params;
64
65
66    /* Over-sampling filter (fractional pitch)*/
67    st->os_fact=4;
68    st->os_filt_ord2=4*st->os_fact;
69    st->os_filt = malloc((1+2*st->os_filt_ord2)*sizeof(float));
70    st->os_filt[st->os_filt_ord2] = 1;
71    for (i=1;i<=st->os_filt_ord2;i++)
72    {
73       float x=M_PI*i/st->os_fact;
74       st->os_filt[st->os_filt_ord2-i] = st->os_filt[st->os_filt_ord2+i]=sin(x)/x*(.5+.5*cos(M_PI*i/st->os_filt_ord2));
75    }
76    /* Normalizing the over-sampling filter */
77    tmp=0;
78    for (i=0;i<2*st->os_filt_ord2+1;i++)
79       tmp += st->os_filt[i];
80    tmp=1/tmp;
81    for (i=0;i<2*st->os_filt_ord2+1;i++)
82       st->os_filt[i] *= tmp;
83
84    /*for (i=0;i<2*st->os_filt_ord2+1;i++)
85       printf ("%f ", st->os_filt[i]);
86       printf ("\n");*/
87
88    /* Allocating input buffer */
89    st->inBuf = calloc(st->bufSize,sizeof(float));
90    st->frame = st->inBuf + st->bufSize - st->windowSize;
91    /* Allocating excitation buffer */
92    st->excBuf = calloc(st->bufSize,sizeof(float));
93    st->exc = st->excBuf + st->bufSize - st->windowSize;
94    st->swBuf = calloc(st->bufSize,sizeof(float));
95    st->sw = st->swBuf + st->bufSize - st->windowSize;
96
97    /* Hanning window */
98    st->window = malloc(st->windowSize*sizeof(float));
99    for (i=0;i<st->windowSize;i++)
100       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
101
102    /* Create the window for autocorrelation (lag-windowing) */
103    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
104    for (i=0;i<st->lpcSize+1;i++)
105       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));
106
107    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
108
109    st->stack = calloc(10000, sizeof(float));
110
111    st->buf2 = malloc(st->windowSize*sizeof(float));
112
113    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
114    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
115    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
116    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
117    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
118
119    st->lsp = malloc(st->lpcSize*sizeof(float));
120    st->qlsp = malloc(st->lpcSize*sizeof(float));
121    st->old_lsp = malloc(st->lpcSize*sizeof(float));
122    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
123    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
124    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
125    st->rc = malloc(st->lpcSize*sizeof(float));
126    st->first = 1;
127    
128    st->mem_sp = calloc(st->lpcSize, sizeof(float));
129    st->mem_sw = calloc(st->lpcSize, sizeof(float));
130 }
131
132 void encoder_destroy(EncState *st)
133 {
134    /* Free all allocated memory */
135    free(st->inBuf);
136    free(st->excBuf);
137    free(st->swBuf);
138    
139    free(st->stack);
140
141    free(st->window);
142    free(st->buf2);
143    free(st->lpc);
144    free(st->interp_lpc);
145    free(st->interp_qlpc);
146
147    free(st->bw_lpc1);
148    free(st->bw_lpc2);
149    free(st->autocorr);
150    free(st->lagWindow);
151    free(st->lsp);
152    free(st->qlsp);
153    free(st->old_lsp);
154    free(st->interp_lsp);
155    free(st->old_qlsp);
156    free(st->interp_qlsp);
157    free(st->rc);
158
159    free(st->mem_sp);
160    free(st->mem_sw);
161 }
162
163 void encode(EncState *st, float *in, FrameBits *bits)
164 {
165    int i, sub, roots;
166    float error;
167
168    /* Copy new data in input buffer */
169    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
170    for (i=0;i<st->frameSize;i++)
171       st->inBuf[st->bufSize-st->frameSize+i] = in[i];
172    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
173    memmove(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
174
175    /* Window for analysis */
176    for (i=0;i<st->windowSize;i++)
177       st->buf2[i] = st->frame[i] * st->window[i];
178
179    /* Compute auto-correlation */
180    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
181
182    st->autocorr[0] += 1;        /* prevents NANs */
183    st->autocorr[0] *= st->lpc_floor; /* Noise floor in auto-correlation domain */
184    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
185    for (i=0;i<st->lpcSize+1;i++)
186       st->autocorr[i] *= st->lagWindow[i];
187
188    /* Levinson-Durbin */
189    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
190    st->lpc[0]=1;
191
192    /* LPC to LSPs (x-domain) transform */
193    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.002, st->stack);
194    if (roots!=st->lpcSize)
195    {
196       fprintf (stderr, "roots!=st->lpcSize (found only %d roots)\n", roots);
197       exit(1);
198    }
199
200    /* x-domain to angle domain*/
201    for (i=0;i<st->lpcSize;i++)
202       st->lsp[i] = acos(st->lsp[i]);
203    
204    /* LSP Quantization */
205    st->lsp_quant(st->lsp, st->qlsp, st->lpcSize, bits);
206    /*for (i=0;i<st->lpcSize;i++)
207      st->qlsp[i]=st->lsp[i];*/
208    /*printf ("LSP ");
209    for (i=0;i<st->lpcSize;i++)
210       printf ("%f ", st->lsp[i]);
211    printf ("\n");
212    printf ("QLSP ");
213    for (i=0;i<st->lpcSize;i++)
214       printf ("%f ", st->qlsp[i]);
215    printf ("\n");
216    return;*/
217    /* Special case for first frame */
218    if (st->first)
219    {
220       for (i=0;i<st->lpcSize;i++)
221          st->old_lsp[i] = st->lsp[i];
222       for (i=0;i<st->lpcSize;i++)
223          st->old_qlsp[i] = st->qlsp[i];
224    }
225
226    /* Loop on sub-frames */
227    for (sub=0;sub<st->nbSubframes;sub++)
228    {
229       float esig, enoise, snr, tmp;
230       int   offset;
231       float *sp, *sw, *res, *exc, *target, *mem;
232       
233       /* Offset relative to start of frame */
234       offset = st->subframeSize*sub;
235       /* Original signal */
236       sp=st->frame+offset;
237       /* Excitation */
238       exc=st->exc+offset;
239       /* Weighted signal */
240       sw=st->sw+offset;
241       /* Filter response */
242       res = PUSH(st->stack, st->subframeSize);
243       /* Target signal */
244       target = PUSH(st->stack, st->subframeSize);
245       mem = PUSH(st->stack, st->lpcSize);
246
247       /* LSP interpolation (quantized and unquantized) */
248       tmp = (.5 + sub)/st->nbSubframes;
249       for (i=0;i<st->lpcSize;i++)
250          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
251       for (i=0;i<st->lpcSize;i++)
252          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
253
254       /* Compute interpolated LPCs (quantized and unquantized) */
255       for (i=0;i<st->lpcSize;i++)
256          st->interp_lsp[i] = cos(st->interp_lsp[i]);
257       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,st->stack);
258
259       for (i=0;i<st->lpcSize;i++)
260          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
261       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
262
263       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
264       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
265       bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
266       
267       /* Reset excitation */
268       for (i=0;i<st->subframeSize;i++)
269          exc[i]=0;
270
271       /* Compute zero response of A(z/g1) / ( A(z/g2) * Aq(z) ) */
272       for (i=0;i<st->lpcSize;i++)
273          mem[i]=st->mem_sp[i];
274       syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, mem);
275       for (i=0;i<st->lpcSize;i++)
276          mem[i]=st->mem_sp[i];
277       residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, mem);
278       for (i=0;i<st->lpcSize;i++)
279          mem[i]=st->mem_sw[i];
280       syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, mem);
281
282       /* Compute weighted signal */
283       for (i=0;i<st->lpcSize;i++)
284          mem[i]=st->mem_sp[i];
285       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
286       for (i=0;i<st->lpcSize;i++)
287          mem[i]=st->mem_sw[i];
288       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
289       
290       esig=0;
291       for (i=0;i<st->subframeSize;i++)
292          esig+=sw[i]*sw[i];
293       
294       /* Compute target signal */
295       for (i=0;i<st->subframeSize;i++)
296          target[i]=sw[i]-res[i];
297
298       for (i=0;i<st->subframeSize;i++)
299          exc[i]=0;
300
301       /* Long-term prediction */
302       st->ltp_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
303                     exc, st->ltp_params, st->min_pitch, st->max_pitch, 
304                     st->lpcSize, st->subframeSize, bits, st->stack);
305
306       /* Update target for adaptive codebook contribution */
307       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
308       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
309       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
310       for (i=0;i<st->subframeSize;i++)
311         target[i]-=res[i];
312
313       /* Compute noise energy and SNR */
314       enoise=0;
315       for (i=0;i<st->subframeSize;i++)
316          enoise += target[i]*target[i];
317       snr = 10*log10((esig+1)/(enoise+1));
318       printf ("pitch SNR = %f\n", snr);
319
320 #if 1 /*If set to 1, compute "real innovation" i.e. cheat to get perfect reconstruction*/
321       syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
322       residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
323       residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
324       if (snr>9 && (rand()%10==0))
325       {
326          printf ("exc ");
327          for (i=0;i<st->subframeSize;i++)
328          {
329             if (i && i%8==0)
330                printf ("\nexc ");
331             printf ("%f ", st->buf2[i]);
332          }
333          printf ("\n");
334       }
335       /*for (i=0;i<st->subframeSize;i++)
336          exc[i]+=st->buf2[i];
337 #else*/
338       /* Perform a split-codebook search */
339       st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
340                            st->innovation_params, st->lpcSize,
341                            st->subframeSize, exc, bits, st->stack);
342 #endif
343       /* Compute weighted noise energy and SNR */
344       enoise=0;
345       for (i=0;i<st->subframeSize;i++)
346          enoise += target[i]*target[i];
347       snr = 10*log10((esig+1)/(enoise+1));
348       printf ("seg SNR = %f\n", snr);
349
350       /*Keep the previous memory*/
351       for (i=0;i<st->lpcSize;i++)
352          mem[i]=st->mem_sp[i];
353       /* Final signal synthesis from excitation */
354       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
355
356       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
357       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, mem);
358       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
359
360       POP(st->stack);
361       POP(st->stack);
362       POP(st->stack);
363    }
364
365    /* Store the LSPs for interpolation in the next frame */
366    for (i=0;i<st->lpcSize;i++)
367       st->old_lsp[i] = st->lsp[i];
368    for (i=0;i<st->lpcSize;i++)
369       st->old_qlsp[i] = st->qlsp[i];
370
371    /* The next frame will not be the first (Duh!) */
372    st->first = 0;
373
374    /* Replace input by synthesized speech */
375    for (i=0;i<st->frameSize;i++)
376      in[i]=st->frame[i];
377 }
378
379
380 void decoder_init(DecState *st, SpeexMode *mode)
381 {
382    int i;
383    /* Codec parameters, should eventually have several "modes"*/
384    st->frameSize = mode->frameSize;
385    st->windowSize = mode->windowSize;
386    st->nbSubframes=mode->frameSize/mode->subframeSize;
387    st->subframeSize=mode->subframeSize;
388    st->lpcSize = mode->lpcSize;
389    st->bufSize = mode->bufSize;
390    st->gamma1=mode->gamma1;
391    st->gamma2=mode->gamma2;
392    st->min_pitch=mode->pitchStart;
393    st->max_pitch=mode->pitchEnd;
394
395    st->lsp_unquant = mode->lsp_unquant;
396    st->ltp_unquant = mode->ltp_unquant;
397    st->ltp_params = mode->ltp_params;
398
399    st->innovation_unquant = mode->innovation_unquant;
400    st->innovation_params = mode->innovation_params;
401
402    st->stack = calloc(10000, sizeof(float));
403
404    st->inBuf = malloc(st->bufSize*sizeof(float));
405    st->frame = st->inBuf + st->bufSize - st->windowSize;
406    st->excBuf = malloc(st->bufSize*sizeof(float));
407    st->exc = st->excBuf + st->bufSize - st->windowSize;
408    for (i=0;i<st->bufSize;i++)
409       st->inBuf[i]=0;
410    for (i=0;i<st->bufSize;i++)
411       st->excBuf[i]=0;
412
413    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
414    st->qlsp = malloc(st->lpcSize*sizeof(float));
415    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
416    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
417    st->mem_sp = calloc(st->lpcSize, sizeof(float));
418
419 }
420
421 void decoder_destroy(DecState *st)
422 {
423    free(st->inBuf);
424    free(st->excBuf);
425    free(st->interp_qlpc);
426    free(st->qlsp);
427    free(st->old_qlsp);
428    free(st->interp_qlsp);
429    free(st->stack);
430    free(st->mem_sp);
431 }
432
433 void decode(DecState *st, FrameBits *bits, float *out)
434 {
435    int i, sub;
436
437    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
438    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
439
440
441    st->lsp_unquant(st->qlsp, st->lpcSize, bits);
442    if (st->first)
443    {
444       for (i=0;i<st->lpcSize;i++)
445          st->old_qlsp[i] = st->qlsp[i];
446    }
447
448    printf ("decode LSPs: ");
449    for (i=0;i<st->lpcSize;i++)
450       printf ("%f ", st->qlsp[i]);
451    printf ("\n");
452
453    /*Loop on subframes */
454    for (sub=0;sub<st->nbSubframes;sub++)
455    {
456       int offset;
457       float *sp, *exc, tmp;
458       
459       /* Offset relative to start of frame */
460       offset = st->subframeSize*sub;
461       /* Original signal */
462       sp=st->frame+offset;
463       /* Excitation */
464       exc=st->exc+offset;
465
466       /* LSP interpolation (quantized and unquantized) */
467       tmp = (.5 + sub)/st->nbSubframes;
468       for (i=0;i<st->lpcSize;i++)
469          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
470
471       /* Compute interpolated LPCs (unquantized) */
472       for (i=0;i<st->lpcSize;i++)
473          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
474       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, st->stack);
475
476       /* Reset excitation */
477       for (i=0;i<st->subframeSize;i++)
478          exc[i]=0;
479
480       /*Adaptive codebook contribution*/
481       st->ltp_unquant(exc, st->min_pitch, st->max_pitch, st->ltp_params, st->subframeSize, bits, st->stack);
482        
483       /*Fixed codebook contribution*/
484       st->innovation_unquant(exc, st->innovation_params, st->subframeSize, bits, st->stack);
485
486       /*Compute decoded signal*/
487       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
488
489    }
490    
491    /*Copy output signal*/
492    for (i=0;i<st->frameSize;i++)
493       out[i]=st->frame[i];
494
495    /* Store the LSPs for interpolation in the next frame */
496    for (i=0;i<st->lpcSize;i++)
497       st->old_qlsp[i] = st->qlsp[i];
498
499    /* The next frame will not be the first (Duh!) */
500    st->first = 0;
501
502 }