3-tap pitch predictor seems to work
[speexdsp.git] / libspeex / speex.c
1 #include <stdlib.h>
2 #include <stdio.h>
3 #include <string.h>
4 #include <math.h>
5 #include "speex.h"
6 #include "lpc.h"
7 #include "lsp.h"
8 #include "ltp.h"
9
10 #ifndef M_PI
11 #define M_PI           3.14159265358979323846  /* pi */
12 #endif
13
14 #define sqr(x) ((x)*(x))
15
16 void encoder_init(EncState *st)
17 {
18    int i;
19    st->frameSize = 160;
20    st->windowSize = 320;
21    st->nbSubframes=4;
22    st->subframeSize=40;
23    st->lpcSize = 10;
24    st->bufSize = 640;
25    st->gamma=.9;
26    st->inBuf = malloc(st->bufSize*sizeof(float));
27    st->frame = st->inBuf + st->bufSize - st->windowSize;
28    st->wBuf = malloc(st->bufSize*sizeof(float));
29    st->wframe = st->wBuf + st->bufSize - st->windowSize;
30    for (i=0;i<st->bufSize;i++)
31       st->inBuf[i]=0;
32    for (i=0;i<st->bufSize;i++)
33       st->wBuf[i]=0;
34    st->window = malloc(st->windowSize*sizeof(float));
35    /* Hanning window */
36    for (i=0;i<st->windowSize;i++)
37       st->window[i]=.5*(1-cos(2*M_PI*i/st->windowSize));
38    st->buf2 = malloc(st->windowSize*sizeof(float));
39    st->lpc = malloc((st->lpcSize+1)*sizeof(float));
40    st->interp_lpc = malloc((st->lpcSize+1)*sizeof(float));
41    st->bw_lpc = malloc((st->lpcSize+1)*sizeof(float));
42    st->autocorr = malloc((st->lpcSize+1)*sizeof(float));
43    /* Create the window for autocorrelation (lag-windowing) */
44    st->lagWindow = malloc((st->lpcSize+1)*sizeof(float));
45    for (i=0;i<st->lpcSize+1;i++)
46       st->lagWindow[i]=exp(-.5*sqr(2*M_PI*.01*i));
47    st->lsp = malloc(st->lpcSize*sizeof(float));
48    st->old_lsp = malloc(st->lpcSize*sizeof(float));
49    st->interp_lsp = malloc(st->lpcSize*sizeof(float));
50    st->rc = malloc(st->lpcSize*sizeof(float));
51    st->first = 1;
52 }
53
54 void encoder_destroy(EncState *st)
55 {
56    free(st->inBuf);
57    free(st->wBuf);
58    free(st->window);
59    free(st->buf2);
60    free(st->lpc);
61    free(st->interp_lpc);
62    free(st->bw_lpc);
63    free(st->autocorr);
64    free(st->lagWindow);
65    free(st->lsp);
66    free(st->old_lsp);
67    free(st->interp_lsp);
68    free(st->rc);
69 }
70
71 void encode(EncState *st, float *in, int *outSize, void *bits)
72 {
73    int i, j, sub, roots;
74    float error;
75
76    /* Copy new data in input buffer */
77    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
78    for (i=0;i<st->frameSize;i++)
79       st->inBuf[st->bufSize-st->frameSize+i] = in[i];
80    memmove(st->wBuf, st->wBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
81
82    /* Window for analysis */
83    for (i=0;i<st->windowSize;i++)
84       st->buf2[i] = st->frame[i] * st->window[i];
85    /* Compute auto-correlation */
86    autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);
87    st->autocorr[0] += 1;        /* prevents NANs */
88    st->autocorr[0] *= 1.0001;   /* 40 dB noise floor */
89    /* Perform lag windowing here, equivalent to filtering in the power-spectrum domain */
90    for (i=0;i<st->lpcSize+1;i++)
91       st->autocorr[i] *= st->lagWindow[i];
92    /* Levinson-Durbin */
93    /*for (i=0;i<st->lpcSize+1;i++)
94       printf("%f ", st->autocorr[i]);
95    printf ("\n");
96    */
97    error = wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);
98    st->lpc[0]=1;
99    /*for (i=0;i<st->lpcSize+1;i++)
100      printf("%f ", st->lpc[i]);
101    printf ("\n");*/
102    printf ("prediction error = %f, R[0] = %f, gain = %f\n", error, st->autocorr[0], st->autocorr[0]/error);
103
104    /* LPC to LSPs (x-domain) transform */
105    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 6, 0.02);
106    if (roots!=st->lpcSize)
107    {
108       fprintf (stderr, "roots!=st->lpcSize\n");
109       exit(1);
110    }
111    for (i=0;i<st->lpcSize;i++)
112       st->lsp[i] = acos(st->lsp[i]);
113    /*for (i=0;i<roots;i++)
114       printf("%f ", st->lsp[i]);
115       printf ("\n\n");*/
116
117    /* Quantize LSPs */
118
119    for (sub=0;sub<st->nbSubframes;sub++)
120    {
121       float tmp, gain[3];
122       int pitch, offset;
123
124       offset = st->subframeSize*sub;
125       /* LSP interpolation */
126       tmp = (.5 + sub)/st->nbSubframes;
127       for (i=0;i<st->lpcSize;i++)
128          st->interp_lsp[i] = (1-tmp)*st->old_lsp[i] + tmp*st->lsp[i];
129
130       /* Compute interpolated LPCs */
131       for (i=0;i<st->lpcSize;i++)
132          st->interp_lsp[i] = cos(st->interp_lsp[i]);
133       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize);
134
135       for (i=0;i<st->lpcSize+1;i++)
136          printf("%f ", st->interp_lpc[i]);
137       printf ("\n");
138       
139
140       /* Compute bandwidth-expanded LPCs for perceptual weighting*/
141       tmp=1;
142       for (i=0;i<st->lpcSize+1;i++)
143       {
144          st->bw_lpc[i] = tmp * st->interp_lpc[i];
145          tmp *= st->gamma;
146       }
147       /*for (i=0;i<st->lpcSize+1;i++)
148          printf("%f ", st->bw_lpc[i]);
149          printf ("\n");*/
150
151       /* Compute perceptualy weighted residue */      
152       for (i=0;i<st->subframeSize;i++)
153       {
154          st->wframe[offset+i]=st->frame[offset+i];
155          for (j=1;j<st->lpcSize+1;j++)
156            st->wframe[offset+i] += st->frame[offset+i-j]*st->bw_lpc[j];
157       }
158
159       /* Find pitch gain and delay */
160       pitch = three_tap_ltp(st->wframe+offset, st->subframeSize, 20, 120, gain);
161       /*pitch = open_loop_ltp(st->wframe+offset, st->subframeSize, 20, 120, gain);*/
162       
163       /*printf ("pitch = %d, gain = %f\n",pitch,gain);*/
164       printf ("pitch = %d, gain = %f %f %f\n",pitch,gain[0], gain[1], gain[2]);
165    }
166
167    printf ("\n");
168    for (i=0;i<st->lpcSize;i++)
169       st->old_lsp[i] = st->lsp[i];
170    st->first = 0;
171
172 }
173
174
175 void decoder_init(DecState *st)
176 {
177 }
178
179 void decoder_destroy(DecState *st)
180 {
181 }
182
183 void decode(DecState *st, float *bits, float *out)
184 {
185 }