Started the decoder part, I think we now update filters in a better way
[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
31 extern float stoc[];
32
33 #ifndef M_PI
34 #define M_PI           3.14159265358979323846  /* pi */
35 #endif
36
37 #define sqr(x) ((x)*(x))
38 #define min(a,b) ((a) < (b) ? (a) : (b))
39
40 void encoder_init(EncState *st)
41 {
42    int i;
43    /* Codec parameters, should eventually have several "modes"*/
44    st->frameSize = 160;
45    st->windowSize = 320;
46    st->nbSubframes=4;
47    st->subframeSize=40;
48    st->lpcSize = 10;
49    st->bufSize = 640;
50    st->gamma1=.9;
51    st->gamma2=.5;
52
53    st->inBuf = malloc(st->bufSize*sizeof(float));
54    st->frame = st->inBuf + st->bufSize - st->windowSize;
55    st->wBuf = malloc(st->bufSize*sizeof(float));
56    st->wframe = st->wBuf + st->bufSize - st->windowSize;
57    st->excBuf = malloc(st->bufSize*sizeof(float));
58    st->exc = st->excBuf + st->bufSize - st->windowSize;
59    st->resBuf = malloc(st->bufSize*sizeof(float));
60    st->res = st->resBuf + st->bufSize - st->windowSize;
61    st->tBuf = malloc(st->bufSize*sizeof(float));
62    st->tframe = st->tBuf + st->bufSize - st->windowSize;
63    for (i=0;i<st->bufSize;i++)
64       st->inBuf[i]=0;
65    for (i=0;i<st->bufSize;i++)
66       st->wBuf[i]=0;
67    for (i=0;i<st->bufSize;i++)
68       st->resBuf[i]=0;
69    for (i=0;i<st->bufSize;i++)
70       st->excBuf[i]=0;
71    for (i=0;i<st->bufSize;i++)
72       st->tBuf[i]=0;
73
74    /* Hanning window */
75    st->window = malloc(st->windowSize*sizeof(float));
76    for (i=0;i<st->windowSize;i++)
77       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
78
79    /* Create the window for autocorrelation (lag-windowing) */
80    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
81    for (i=0;i<st->lpcSize+1;i++)
82       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*.01*i));
83
84    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
85
86    st->buf2 = malloc(st->windowSize*sizeof(float));
87
88    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
89    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
90    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
91    st->bw_lpc1 = malloc((st->lpcSize+1)*sizeof(float));
92    st->bw_lpc2 = malloc((st->lpcSize+1)*sizeof(float));
93    st->bw_az = malloc((st->lpcSize*2+1)*sizeof(float));
94
95    st->lsp = malloc(st->lpcSize*sizeof(float));
96    st->qlsp = malloc(st->lpcSize*sizeof(float));
97    st->old_lsp = malloc(st->lpcSize*sizeof(float));
98    st->old_qlsp = malloc(st->lpcSize*sizeof(float));
99    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
100    st->interp_qlsp = malloc(st->lpcSize*sizeof(float));
101    st->rc = malloc(st->lpcSize*sizeof(float));
102    st->first = 1;
103    
104    st->mem1 = calloc(st->lpcSize, sizeof(float));
105    st->mem2 = calloc(st->lpcSize, sizeof(float));
106    st->mem3 = calloc(st->lpcSize, sizeof(float));
107    st->mem4 = calloc(st->lpcSize, sizeof(float));
108    st->mem5 = calloc(st->lpcSize, sizeof(float));
109    st->mem6 = calloc(st->lpcSize, sizeof(float));
110    st->mem7 = calloc(st->lpcSize, sizeof(float));
111 }
112
113 void encoder_destroy(EncState *st)
114 {
115    /* Free all allocated memory */
116    free(st->inBuf);
117    free(st->wBuf);
118    free(st->resBuf);
119    free(st->excBuf);
120    free(st->tBuf);
121
122    free(st->window);
123    free(st->buf2);
124    free(st->lpc);
125    free(st->interp_lpc);
126    free(st->interp_qlpc);
127
128    free(st->bw_lpc1);
129    free(st->bw_lpc2);
130    free(st->bw_az);
131    free(st->autocorr);
132    free(st->lagWindow);
133    free(st->lsp);
134    free(st->qlsp);
135    free(st->old_lsp);
136    free(st->interp_lsp);
137    free(st->old_qlsp);
138    free(st->interp_qlsp);
139    free(st->rc);
140
141    free(st->mem1);
142    free(st->mem2);
143    free(st->mem3);
144    free(st->mem4);
145    free(st->mem5);
146    free(st->mem6);
147    free(st->mem7);
148 }
149
150 void encode(EncState *st, float *in, int *outSize, void *bits)
151 {
152    int i, j, sub, roots;
153    float error;
154
155    /* Copy new data in input buffer */
156    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
157    for (i=0;i<st->frameSize;i++)
158       st->inBuf[st->bufSize-st->frameSize+i] = in[i];
159    memmove(st->wBuf, st->wBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
160    memmove(st->resBuf, st->resBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
161    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
162    memmove(st->tBuf, st->tBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
163
164    /* Window for analysis */
165    for (i=0;i<st->windowSize;i++)
166       st->buf2[i] = st->frame[i] * st->window[i];
167
168    /* Compute auto-correlation */
169    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
170
171    st->autocorr[0] += 1;        /* prevents NANs */
172    st->autocorr[0] *= 1.0001;   /* 40 dB noise floor */
173    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
174    for (i=0;i<st->lpcSize+1;i++)
175       st->autocorr[i] *= st->lagWindow[i];
176
177    /* Levinson-Durbin */
178    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
179    st->lpc[0]=1;
180
181    /* LPC to LSPs (x-domain) transform */
182    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.02);
183    if (roots!=st->lpcSize)
184    {
185       fprintf (stderr, "roots!=st->lpcSize\n");
186       exit(1);
187    }
188
189    /* x-domain to angle domain*/
190    for (i=0;i<st->lpcSize;i++)
191       st->lsp[i] = acos(st->lsp[i]);
192    
193    /* LSP Quantization */
194    {
195       unsigned int id;
196       for (i=0;i<st->lpcSize;i++)
197          st->buf2[i]=st->lsp[i];
198       id=lsp_quant_nb(st->buf2,10 );
199       lsp_unquant_nb(st->qlsp,10,id);
200    }
201
202    /* Loop on sub-frames */
203    for (sub=0;sub<st->nbSubframes;sub++)
204    {
205       float tmp, tmp1,tmp2,gain[3];
206       int pitch, offset;
207       float *sp, *sw, *res, *exc, *target;
208       
209       /* Offset relative to start of frame */
210       offset = st->subframeSize*sub;
211       sp=st->frame+offset;
212       sw=st->wframe+offset;
213       res=st->res+offset;
214       exc=st->exc+offset;
215       target=st->tframe+offset;
216
217       /* LSP interpolation (quantized and unquantized) */
218       tmp = (.5 + sub)/st->nbSubframes;
219       for (i=0;i<st->lpcSize;i++)
220          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
221       for (i=0;i<st->lpcSize;i++)
222          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
223
224       /* Compute interpolated LPCs (quantized and unquantized) */
225       for (i=0;i<st->lpcSize;i++)
226          st->interp_lsp[i] = cos(st->interp_lsp[i]);
227       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize);
228
229       for (i=0;i<st->lpcSize;i++)
230          st->interp_qlsp[i] = cos(st->interp_qlsp[i]);
231       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize);
232
233       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
234       tmp=1;
235       for (i=0;i<st->lpcSize+1;i++)
236       {
237          st->bw_lpc1[i] = tmp * st->interp_lpc[i];
238          tmp *= st->gamma1;
239       }
240       tmp=1;
241       for (i=0;i<st->lpcSize+1;i++)
242       {
243          st->bw_lpc2[i] = tmp * st->interp_lpc[i];
244          tmp *= st->gamma2;
245       }
246       
247       /* Reset excitation */
248       for (i=0;i<st->subframeSize;i++)
249          exc[i]=0;
250
251       /* Compute zero response of A(z/g1) / ( A(z/g2) * Aq(z) ) */
252       for (i=0;i<st->lpcSize;i++)
253          st->mem4[i]=st->mem5[i];
254       syn_filt_mem(exc, st->interp_qlpc, exc, st->subframeSize, st->lpcSize, st->mem4);
255       for (i=0;i<st->lpcSize;i++)
256          st->mem4[i]=st->mem5[i];
257       residue_mem(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize, st->mem4);
258       for (i=0;i<st->lpcSize;i++)
259          st->mem4[i]=st->mem2[i];
260       syn_filt_mem(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize, st->mem4);
261
262       /* Compute weighted signal */
263       residue(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize);
264       for (i=0;i<st->lpcSize;i++)
265          st->mem4[i]=st->mem2[i];
266       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem4);
267
268       /*for (i=0;i<st->lpcSize;i++)
269         st->mem3[i]=sw[st->subframeSize-i-1];*/
270
271       /* Compute target signal */
272       for (i=0;i<st->subframeSize;i++)
273          target[i]=sw[i]-res[i];
274
275       for (i=0;i<st->subframeSize;i++)
276          exc[i]=0;
277 #if 1 /*If set to 0, we compute the excitation directly from the target, i.e. we're cheating */
278
279       /* Perform adaptive codebook search (3-tap pitch predictor) */
280       pitch_search_3tap(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
281                         exc, 20, 147, &gain[0], &pitch, st->lpcSize,
282                         st->subframeSize);
283       for (i=0;i<st->subframeSize;i++)
284         exc[i]=gain[0]*exc[i-pitch]+gain[1]*exc[i-pitch-1]+gain[2]*exc[i-pitch-2];
285       printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
286
287       /* Update target for adaptive codebook contribution */
288       residue_zero(exc, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
289       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
290       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
291       for (i=0;i<st->subframeSize;i++)
292         target[i]-=res[i];
293
294       /* Perform stochastic codebook search */
295       overlap_cb_search(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
296                         stoc, 1024, &gain[0], &pitch, st->lpcSize,
297                         st->subframeSize);
298       printf ("gain = %f, index = %d\n",gain[0], pitch);
299       for (i=0;i<st->subframeSize;i++)
300          exc[i]+=gain[0]*stoc[i+pitch];
301
302       /* Update target for adaptive codebook contribution (Useless for now)*/
303       residue_zero(stoc+pitch, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
304       syn_filt_zero(res, st->interp_qlpc, res, st->subframeSize, st->lpcSize);
305       syn_filt_zero(res, st->bw_lpc2, res, st->subframeSize, st->lpcSize);
306       for (i=0;i<st->subframeSize;i++)
307          target[i]-=gain[0]*res[i];
308
309
310 #else
311       /* We're cheating to get perfect reconstruction */
312       syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
313       residue_zero(res, st->interp_qlpc, exc, st->subframeSize, st->lpcSize);
314       residue_zero(exc, st->bw_lpc2, exc, st->subframeSize, st->lpcSize);
315 #endif
316
317       /* Final signal synthesis from excitation */
318       syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem5);
319
320       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
321       residue_mem(sp, st->bw_lpc1, sw, st->subframeSize, st->lpcSize, st->mem1);
322       syn_filt_mem(sw, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem2);
323    }
324
325    /* Store the LSPs for interpolation in the next frame */
326    for (i=0;i<st->lpcSize;i++)
327       st->old_lsp[i] = st->lsp[i];
328    for (i=0;i<st->lpcSize;i++)
329       st->old_qlsp[i] = st->qlsp[i];
330
331    /* The next frame will not by the first (Duh!) */
332    st->first = 0;
333
334    /* Replace input by synthesized speech */
335    for (i=0;i<st->frameSize;i++)
336      in[i]=st->frame[i];
337 }
338
339
340 void decoder_init(DecState *st)
341 {
342 }
343
344 void decoder_destroy(DecState *st)
345 {
346 }
347
348 void decode(DecState *st, float *bits, float *out)
349 {
350 }