The temp stack is now void* instead of float*
[speexdsp.git] / libspeex / cb_search.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: cb_search.c
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32
33 #include <stdlib.h>
34 #include "cb_search.h"
35 #include "filters.h"
36 #include "stack_alloc.h"
37 #include "vq.h"
38
39
40 void split_cb_search_shape_sign(
41 float target[],                 /* target vector */
42 float ak[],                     /* LPCs for this subframe */
43 float awk1[],                   /* Weighted LPCs for this subframe */
44 float awk2[],                   /* Weighted LPCs for this subframe */
45 void *par,                      /* Codebook/search parameters*/
46 int   p,                        /* number of LPC coeffs */
47 int   nsf,                      /* number of samples in subframe */
48 float *exc,
49 float *r,
50 SpeexBits *bits,
51 void *stack,
52 int   complexity
53 )
54 {
55    int i,j,k,m,n,q;
56    float *resp;
57    float *t, *e, *E, *r2;
58    /*FIXME: Should make this dynamic*/
59    float *tmp, *_ot[20], *_nt[20];
60    float *ndist, *odist;
61    int *itmp, *_nind[20], *_oind[20];
62    float **ot, **nt;
63    int **nind, **oind;
64    int *ind;
65    float *shape_cb;
66    int shape_cb_size, subvect_size, nb_subvect;
67    split_cb_params *params;
68    int N=2;
69    int *best_index;
70    float *best_dist;
71    int have_sign;
72
73    ot=_ot;
74    nt=_nt;
75    oind=_oind;
76    nind=_nind;
77    N=complexity;
78    if (N>10)
79       N=10;
80
81    params = (split_cb_params *) par;
82    subvect_size = params->subvect_size;
83    nb_subvect = params->nb_subvect;
84    shape_cb_size = 1<<params->shape_bits;
85    shape_cb = params->shape_cb;
86    have_sign = params->have_sign;
87    resp = PUSH(stack, shape_cb_size*subvect_size, float);
88    t = PUSH(stack, nsf, float);
89    e = PUSH(stack, nsf, float);
90    r2 = PUSH(stack, nsf, float);
91    E = PUSH(stack, shape_cb_size, float);
92    ind = PUSH(stack, nb_subvect, int);
93
94    tmp = PUSH(stack, 2*N*nsf, float);
95    for (i=0;i<N;i++)
96    {
97       ot[i]=tmp;
98       tmp += nsf;
99       nt[i]=tmp;
100       tmp += nsf;
101    }
102
103    best_index = PUSH(stack, N, int);
104    best_dist = PUSH(stack, N, float);
105    ndist = PUSH(stack, N, float);
106    odist = PUSH(stack, N, float);
107    
108    itmp = PUSH(stack, 2*N*nb_subvect, int);
109    for (i=0;i<N;i++)
110    {
111       nind[i]=itmp;
112       itmp+=nb_subvect;
113       oind[i]=itmp;
114       itmp+=nb_subvect;
115       for (j=0;j<nb_subvect;j++)
116          nind[i][j]=oind[i][j]=-1;
117    }
118
119    for (j=0;j<N;j++)
120       for (i=0;i<nsf;i++)
121          ot[j][i]=target[i];
122
123    for (i=0;i<nsf;i++)
124       t[i]=target[i];
125
126    /* Pre-compute codewords response and energy */
127    for (i=0;i<shape_cb_size;i++)
128    {
129       float *res;
130       float *shape;
131
132       res = resp+i*subvect_size;
133       shape = shape_cb+i*subvect_size;
134
135       /* Compute codeword response using convolution with impulse response */
136       for(j=0;j<subvect_size;j++)
137       {
138          res[j]=0;
139          for (k=0;k<=j;k++)
140             res[j] += shape[k]*r[j-k];
141       }
142       
143       /* Compute codeword energy */
144       E[i]=0;
145       for(j=0;j<subvect_size;j++)
146          E[i]+=res[j]*res[j];
147    }
148
149    for (j=0;j<N;j++)
150       odist[j]=0;
151    /*For all subvectors*/
152    for (i=0;i<nb_subvect;i++)
153    {
154       /*"erase" nbest list*/
155       for (j=0;j<N;j++)
156          ndist[j]=-1;
157
158       /*For all n-bests of previous subvector*/
159       for (j=0;j<N;j++)
160       {
161          float *x=ot[j]+subvect_size*i;
162          /*Find new n-best based on previous n-best j*/
163          if (have_sign)
164             vq_nbest_sign(x, resp, subvect_size, shape_cb_size, E, N, best_index, best_dist);
165          else
166             vq_nbest(x, resp, subvect_size, shape_cb_size, E, N, best_index, best_dist);
167
168          /*For all new n-bests*/
169          for (k=0;k<N;k++)
170          {
171             float *ct;
172             float err=0;
173             ct = ot[j];
174             /*update target*/
175
176             /*previous target*/
177             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
178                t[m]=ct[m];
179
180             /* New code: update only enough of the target to calculate error*/
181             {
182                int rind;
183                float *res;
184                float sign=1;
185                rind = best_index[k];
186                if (rind>shape_cb_size)
187                {
188                   sign=-1;
189                   rind-=shape_cb_size;
190                }
191                res = resp+rind*subvect_size;
192                if (sign>0)
193                   for (m=0;m<subvect_size;m++)
194                      t[subvect_size*i+m] -= res[m];
195                else
196                   for (m=0;m<subvect_size;m++)
197                      t[subvect_size*i+m] += res[m];
198             }
199             
200             /*compute error (distance)*/
201             err=odist[j];
202             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
203                err += t[m]*t[m];
204             /*update n-best list*/
205             if (err<ndist[N-1] || ndist[N-1]<-.5)
206             {
207
208                /*previous target (we don't care what happened before*/
209                for (m=(i+1)*subvect_size;m<nsf;m++)
210                   t[m]=ct[m];
211                /* New code: update the rest of the target only if it's worth it */
212                for (m=0;m<subvect_size;m++)
213                {
214                   float g;
215                   int rind;
216                   float sign=1;
217                   rind = best_index[k];
218                   if (rind>shape_cb_size)
219                   {
220                      sign=-1;
221                      rind-=shape_cb_size;
222                   }
223
224                   g=sign*shape_cb[rind*subvect_size+m];
225                   q=subvect_size-m;
226                   for (n=subvect_size*(i+1);n<nsf;n++,q++)
227                      t[n] -= g*r[q];
228                }
229
230
231                for (m=0;m<N;m++)
232                {
233                   if (err < ndist[m] || ndist[m]<-.5)
234                   {
235                      for (n=N-1;n>m;n--)
236                      {
237                         for (q=(i+1)*subvect_size;q<nsf;q++)
238                            nt[n][q]=nt[n-1][q];
239                         for (q=0;q<nb_subvect;q++)
240                            nind[n][q]=nind[n-1][q];
241                         ndist[n]=ndist[n-1];
242                      }
243                      for (q=(i+1)*subvect_size;q<nsf;q++)
244                         nt[m][q]=t[q];
245                      for (q=0;q<nb_subvect;q++)
246                         nind[m][q]=oind[j][q];
247                      nind[m][i]=best_index[k];
248                      ndist[m]=err;
249                      break;
250                   }
251                }
252             }
253          }
254          if (i==0)
255            break;
256       }
257
258       /*update old-new data*/
259       /* just swap pointers instead of a long copy */
260       {
261          float **tmp;
262          tmp=ot;
263          ot=nt;
264          nt=tmp;
265       }
266       for (j=0;j<N;j++)
267          for (m=0;m<nb_subvect;m++)
268             oind[j][m]=nind[j][m];
269       for (j=0;j<N;j++)
270          odist[j]=ndist[j];
271    }
272
273    /*save indices*/
274    for (i=0;i<nb_subvect;i++)
275    {
276       ind[i]=nind[0][i];
277       speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
278    }
279    
280    /* Put everything back together */
281    for (i=0;i<nb_subvect;i++)
282    {
283       int rind;
284       float sign=1;
285       rind = ind[i];
286       if (rind>shape_cb_size)
287       {
288          sign=-1;
289          rind-=shape_cb_size;
290       }
291
292       for (j=0;j<subvect_size;j++)
293          e[subvect_size*i+j]=sign*shape_cb[rind*subvect_size+j];
294    }   
295    /* Update excitation */
296    for (j=0;j<nsf;j++)
297       exc[j]+=e[j];
298    
299    /* Update target */
300    syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
301    for (j=0;j<nsf;j++)
302       target[j]-=r2[j];
303
304 }
305
306
307 void split_cb_shape_sign_unquant(
308 float *exc,
309 void *par,                      /* non-overlapping codebook */
310 int   nsf,                      /* number of samples in subframe */
311 SpeexBits *bits,
312 void *stack
313 )
314 {
315    int i,j;
316    int *ind, *signs;
317    float *shape_cb;
318    int shape_cb_size, subvect_size, nb_subvect;
319    split_cb_params *params;
320    int have_sign;
321
322    params = (split_cb_params *) par;
323    subvect_size = params->subvect_size;
324    nb_subvect = params->nb_subvect;
325    shape_cb_size = 1<<params->shape_bits;
326    shape_cb = params->shape_cb;
327    have_sign = params->have_sign;
328
329    ind = PUSH(stack, nb_subvect, int);
330    signs = PUSH(stack, nb_subvect, int);
331
332    /* Decode codewords and gains */
333    for (i=0;i<nb_subvect;i++)
334    {
335       if (have_sign)
336          signs[i] = speex_bits_unpack_unsigned(bits, 1);
337       else
338          signs[i] = 0;
339       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
340    }
341    /* Compute decoded excitation */
342    for (i=0;i<nb_subvect;i++)
343    {
344       float s=1;
345       if (signs[i])
346          s=-1;
347       for (j=0;j<subvect_size;j++)
348          exc[subvect_size*i+j]+=s*shape_cb[ind[i]*subvect_size+j];
349    }
350 }
351
352 void noise_codebook_quant(
353 float target[],                 /* target vector */
354 float ak[],                     /* LPCs for this subframe */
355 float awk1[],                   /* Weighted LPCs for this subframe */
356 float awk2[],                   /* Weighted LPCs for this subframe */
357 void *par,                      /* Codebook/search parameters*/
358 int   p,                        /* number of LPC coeffs */
359 int   nsf,                      /* number of samples in subframe */
360 float *exc,
361 float *r,
362 SpeexBits *bits,
363 void *stack,
364 int   complexity
365 )
366 {
367    int i;
368    float *tmp=PUSH(stack, nsf, float);
369    residue_percep_zero(target, ak, awk1, awk2, tmp, nsf, p, stack);
370
371    for (i=0;i<nsf;i++)
372       exc[i]+=tmp[i];
373    for (i=0;i<nsf;i++)
374       target[i]=0;
375
376 }
377
378
379 void noise_codebook_unquant(
380 float *exc,
381 void *par,                      /* non-overlapping codebook */
382 int   nsf,                      /* number of samples in subframe */
383 SpeexBits *bits,
384 void *stack
385 )
386 {
387    int i;
388
389    for (i=0;i<nsf;i++)
390       exc[i]+=3*((((float)rand())/RAND_MAX)-.5);
391 }