Removed some gcc warnings
[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 "cb_search.h"
34 #include "filters.h"
35 #include "stack_alloc.h"
36 #include "vq.h"
37 #include "misc.h"
38
39 void split_cb_search_shape_sign(
40 float target[],                 /* target vector */
41 float ak[],                     /* LPCs for this subframe */
42 float awk1[],                   /* Weighted LPCs for this subframe */
43 float awk2[],                   /* Weighted LPCs for this subframe */
44 void *par,                      /* Codebook/search parameters*/
45 int   p,                        /* number of LPC coeffs */
46 int   nsf,                      /* number of samples in subframe */
47 float *exc,
48 float *r,
49 SpeexBits *bits,
50 char *stack,
51 int   complexity
52 )
53 {
54    int i,j,k,m,n,q;
55    float *resp;
56    float *t, *e, *E, *r2;
57    float *tmp;
58    float *ndist, *odist;
59    int *itmp;
60    float **ot, **nt;
61    int **nind, **oind;
62    int *ind;
63    signed char *shape_cb;
64    int shape_cb_size, subvect_size, nb_subvect;
65    split_cb_params *params;
66    int N=2;
67    int *best_index;
68    float *best_dist;
69    int have_sign;
70
71    N=complexity;
72    if (N>10)
73       N=10;
74
75    ot=PUSH(stack, N, float*);
76    nt=PUSH(stack, N, float*);
77    oind=PUSH(stack, N, int*);
78    nind=PUSH(stack, N, int*);
79
80    params = (split_cb_params *) par;
81    subvect_size = params->subvect_size;
82    nb_subvect = params->nb_subvect;
83    shape_cb_size = 1<<params->shape_bits;
84    shape_cb = params->shape_cb;
85    have_sign = params->have_sign;
86    resp = PUSH(stack, shape_cb_size*subvect_size, float);
87    t = PUSH(stack, nsf, float);
88    e = PUSH(stack, nsf, float);
89    r2 = PUSH(stack, nsf, float);
90    E = PUSH(stack, shape_cb_size, float);
91    ind = PUSH(stack, nb_subvect, int);
92
93    tmp = PUSH(stack, 2*N*nsf, float);
94    for (i=0;i<N;i++)
95    {
96       ot[i]=tmp;
97       tmp += nsf;
98       nt[i]=tmp;
99       tmp += nsf;
100    }
101
102    best_index = PUSH(stack, N, int);
103    best_dist = PUSH(stack, N, float);
104    ndist = PUSH(stack, N, float);
105    odist = PUSH(stack, N, float);
106    
107    itmp = PUSH(stack, 2*N*nb_subvect, int);
108    for (i=0;i<N;i++)
109    {
110       nind[i]=itmp;
111       itmp+=nb_subvect;
112       oind[i]=itmp;
113       itmp+=nb_subvect;
114       for (j=0;j<nb_subvect;j++)
115          nind[i][j]=oind[i][j]=-1;
116    }
117
118    for (j=0;j<N;j++)
119       for (i=0;i<nsf;i++)
120          ot[j][i]=target[i];
121
122    for (i=0;i<nsf;i++)
123       t[i]=target[i];
124
125    /* Pre-compute codewords response and energy */
126    for (i=0;i<shape_cb_size;i++)
127    {
128       float *res;
129       signed char *shape;
130
131       res = resp+i*subvect_size;
132       shape = shape_cb+i*subvect_size;
133
134       /* Compute codeword response using convolution with impulse response */
135       for(j=0;j<subvect_size;j++)
136       {
137          res[j]=0;
138          for (k=0;k<=j;k++)
139             res[j] += 0.03125*shape[k]*r[j-k];
140       }
141       
142       /* Compute codeword energy */
143       E[i]=0;
144       for(j=0;j<subvect_size;j++)
145          E[i]+=res[j]*res[j];
146    }
147
148    for (j=0;j<N;j++)
149       odist[j]=0;
150    /*For all subvectors*/
151    for (i=0;i<nb_subvect;i++)
152    {
153       /*"erase" nbest list*/
154       for (j=0;j<N;j++)
155          ndist[j]=-1;
156
157       /*For all n-bests of previous subvector*/
158       for (j=0;j<N;j++)
159       {
160          float *x=ot[j]+subvect_size*i;
161          /*Find new n-best based on previous n-best j*/
162          if (have_sign)
163             vq_nbest_sign(x, resp, subvect_size, shape_cb_size, E, N, best_index, best_dist);
164          else
165             vq_nbest(x, resp, subvect_size, shape_cb_size, E, N, best_index, best_dist);
166
167          /*For all new n-bests*/
168          for (k=0;k<N;k++)
169          {
170             float *ct;
171             float err=0;
172             ct = ot[j];
173             /*update target*/
174
175             /*previous target*/
176             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
177                t[m]=ct[m];
178
179             /* New code: update only enough of the target to calculate error*/
180             {
181                int rind;
182                float *res;
183                float sign=1;
184                rind = best_index[k];
185                if (rind>=shape_cb_size)
186                {
187                   sign=-1;
188                   rind-=shape_cb_size;
189                }
190                res = resp+rind*subvect_size;
191                if (sign>0)
192                   for (m=0;m<subvect_size;m++)
193                      t[subvect_size*i+m] -= res[m];
194                else
195                   for (m=0;m<subvect_size;m++)
196                      t[subvect_size*i+m] += res[m];
197             }
198             
199             /*compute error (distance)*/
200             err=odist[j];
201             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
202                err += t[m]*t[m];
203             /*update n-best list*/
204             if (err<ndist[N-1] || ndist[N-1]<-.5)
205             {
206
207                /*previous target (we don't care what happened before*/
208                for (m=(i+1)*subvect_size;m<nsf;m++)
209                   t[m]=ct[m];
210                /* New code: update the rest of the target only if it's worth it */
211                for (m=0;m<subvect_size;m++)
212                {
213                   float g;
214                   int rind;
215                   float sign=1;
216                   rind = best_index[k];
217                   if (rind>=shape_cb_size)
218                   {
219                      sign=-1;
220                      rind-=shape_cb_size;
221                   }
222
223                   g=sign*0.03125*shape_cb[rind*subvect_size+m];
224                   q=subvect_size-m;
225                   for (n=subvect_size*(i+1);n<nsf;n++,q++)
226                      t[n] -= g*r[q];
227                }
228
229
230                for (m=0;m<N;m++)
231                {
232                   if (err < ndist[m] || ndist[m]<-.5)
233                   {
234                      for (n=N-1;n>m;n--)
235                      {
236                         for (q=(i+1)*subvect_size;q<nsf;q++)
237                            nt[n][q]=nt[n-1][q];
238                         for (q=0;q<nb_subvect;q++)
239                            nind[n][q]=nind[n-1][q];
240                         ndist[n]=ndist[n-1];
241                      }
242                      for (q=(i+1)*subvect_size;q<nsf;q++)
243                         nt[m][q]=t[q];
244                      for (q=0;q<nb_subvect;q++)
245                         nind[m][q]=oind[j][q];
246                      nind[m][i]=best_index[k];
247                      ndist[m]=err;
248                      break;
249                   }
250                }
251             }
252          }
253          if (i==0)
254            break;
255       }
256
257       /*update old-new data*/
258       /* just swap pointers instead of a long copy */
259       {
260          float **tmp2;
261          tmp2=ot;
262          ot=nt;
263          nt=tmp2;
264       }
265       for (j=0;j<N;j++)
266          for (m=0;m<nb_subvect;m++)
267             oind[j][m]=nind[j][m];
268       for (j=0;j<N;j++)
269          odist[j]=ndist[j];
270    }
271
272    /*save indices*/
273    for (i=0;i<nb_subvect;i++)
274    {
275       ind[i]=nind[0][i];
276       speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
277    }
278    
279    /* Put everything back together */
280    for (i=0;i<nb_subvect;i++)
281    {
282       int rind;
283       float sign=1;
284       rind = ind[i];
285       if (rind>=shape_cb_size)
286       {
287          sign=-1;
288          rind-=shape_cb_size;
289       }
290
291       for (j=0;j<subvect_size;j++)
292          e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
293    }   
294    /* Update excitation */
295    for (j=0;j<nsf;j++)
296       exc[j]+=e[j];
297    
298    /* Update target */
299    syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
300    for (j=0;j<nsf;j++)
301       target[j]-=r2[j];
302
303 }
304
305
306 void split_cb_shape_sign_unquant(
307 float *exc,
308 void *par,                      /* non-overlapping codebook */
309 int   nsf,                      /* number of samples in subframe */
310 SpeexBits *bits,
311 char *stack
312 )
313 {
314    int i,j;
315    int *ind, *signs;
316    signed char *shape_cb;
317    int shape_cb_size, subvect_size, nb_subvect;
318    split_cb_params *params;
319    int have_sign;
320
321    params = (split_cb_params *) par;
322    subvect_size = params->subvect_size;
323    nb_subvect = params->nb_subvect;
324    shape_cb_size = 1<<params->shape_bits;
325    shape_cb = params->shape_cb;
326    have_sign = params->have_sign;
327
328    ind = PUSH(stack, nb_subvect, int);
329    signs = PUSH(stack, nb_subvect, int);
330
331    /* Decode codewords and gains */
332    for (i=0;i<nb_subvect;i++)
333    {
334       if (have_sign)
335          signs[i] = speex_bits_unpack_unsigned(bits, 1);
336       else
337          signs[i] = 0;
338       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
339    }
340    /* Compute decoded excitation */
341    for (i=0;i<nb_subvect;i++)
342    {
343       float s=1;
344       if (signs[i])
345          s=-1;
346       for (j=0;j<subvect_size;j++)
347          exc[subvect_size*i+j]+=s*0.03125*shape_cb[ind[i]*subvect_size+j];
348    }
349 }
350
351 void noise_codebook_quant(
352 float target[],                 /* target vector */
353 float ak[],                     /* LPCs for this subframe */
354 float awk1[],                   /* Weighted LPCs for this subframe */
355 float awk2[],                   /* Weighted LPCs for this subframe */
356 void *par,                      /* Codebook/search parameters*/
357 int   p,                        /* number of LPC coeffs */
358 int   nsf,                      /* number of samples in subframe */
359 float *exc,
360 float *r,
361 SpeexBits *bits,
362 char *stack,
363 int   complexity
364 )
365 {
366    int i;
367    float *tmp=PUSH(stack, nsf, float);
368    residue_percep_zero(target, ak, awk1, awk2, tmp, nsf, p, stack);
369
370    for (i=0;i<nsf;i++)
371       exc[i]+=tmp[i];
372    for (i=0;i<nsf;i++)
373       target[i]=0;
374
375 }
376
377
378 void noise_codebook_unquant(
379 float *exc,
380 void *par,                      /* non-overlapping codebook */
381 int   nsf,                      /* number of samples in subframe */
382 SpeexBits *bits,
383 char *stack
384 )
385 {
386    speex_rand_vec(1, exc, nsf);
387 }