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