d4e00b4e986c3aa86c1954150aa2058afed841f4
[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 const 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    const 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       const 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 = MAC16_16_Q11(resj,shape[k],r[j-k]);
147 #ifndef FIXED_POINT
148          resj *= 0.03125;
149 #endif
150          res[j] = resj;
151          /*printf ("%d\n", (int)res[j]);*/
152       }
153       
154       /* Compute codeword energy */
155       E[i]=0;
156       for(j=0;j<subvect_size;j++)
157          E[i]=ADD32(E[i],MULT16_16(res[j],res[j]));
158    }
159
160    for (j=0;j<N;j++)
161       odist[j]=0;
162    /*For all subvectors*/
163    for (i=0;i<nb_subvect;i++)
164    {
165       /*"erase" nbest list*/
166       for (j=0;j<N;j++)
167          ndist[j]=-2;
168
169       /*For all n-bests of previous subvector*/
170       for (j=0;j<N;j++)
171       {
172          spx_word16_t *x=ot[j]+subvect_size*i;
173          /*Find new n-best based on previous n-best j*/
174          if (have_sign)
175             vq_nbest_sign(x, resp, subvect_size, shape_cb_size, E, N, best_index, best_dist);
176          else
177             vq_nbest(x, resp, subvect_size, shape_cb_size, E, N, best_index, best_dist);
178
179          /*For all new n-bests*/
180          for (k=0;k<N;k++)
181          {
182             spx_word16_t *ct;
183             spx_word32_t err=0;
184             ct = ot[j];
185             /*update target*/
186
187             /*previous target*/
188             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
189                t[m]=ct[m];
190
191             /* New code: update only enough of the target to calculate error*/
192             {
193                int rind;
194                spx_word16_t *res;
195                spx_word16_t sign=1;
196                rind = best_index[k];
197                if (rind>=shape_cb_size)
198                {
199                   sign=-1;
200                   rind-=shape_cb_size;
201                }
202                res = resp+rind*subvect_size;
203                if (sign>0)
204                   for (m=0;m<subvect_size;m++)
205                      t[subvect_size*i+m] -= res[m];
206                else
207                   for (m=0;m<subvect_size;m++)
208                      t[subvect_size*i+m] += res[m];
209             }
210             
211             /*compute error (distance)*/
212             err=odist[j];
213             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
214                err += t[m]*t[m];
215             /*update n-best list*/
216             if (err<ndist[N-1] || ndist[N-1]<-1)
217             {
218
219                /*previous target (we don't care what happened before*/
220                for (m=(i+1)*subvect_size;m<nsf;m++)
221                   t[m]=ct[m];
222                /* New code: update the rest of the target only if it's worth it */
223                for (m=0;m<subvect_size;m++)
224                {
225                   spx_word16_t g;
226                   int rind;
227                   spx_word16_t sign=1;
228                   rind = best_index[k];
229                   if (rind>=shape_cb_size)
230                   {
231                      sign=-1;
232                      rind-=shape_cb_size;
233                   }
234
235                   q=subvect_size-m;
236 #ifdef FIXED_POINT
237                   g=sign*shape_cb[rind*subvect_size+m];
238                   for (n=subvect_size*(i+1);n<nsf;n++,q++)
239                      t[n] = SUB32(t[n],MULT16_16_Q11(g,r[q]));
240 #else
241                   g=sign*0.03125*shape_cb[rind*subvect_size+m];
242                   for (n=subvect_size*(i+1);n<nsf;n++,q++)
243                      t[n] = SUB32(t[n],g*r[q]);
244 #endif
245                }
246
247
248                for (m=0;m<N;m++)
249                {
250                   if (err < ndist[m] || ndist[m]<-1)
251                   {
252                      for (n=N-1;n>m;n--)
253                      {
254                         for (q=(i+1)*subvect_size;q<nsf;q++)
255                            nt[n][q]=nt[n-1][q];
256                         for (q=0;q<nb_subvect;q++)
257                            nind[n][q]=nind[n-1][q];
258                         ndist[n]=ndist[n-1];
259                      }
260                      for (q=(i+1)*subvect_size;q<nsf;q++)
261                         nt[m][q]=t[q];
262                      for (q=0;q<nb_subvect;q++)
263                         nind[m][q]=oind[j][q];
264                      nind[m][i]=best_index[k];
265                      ndist[m]=err;
266                      break;
267                   }
268                }
269             }
270          }
271          if (i==0)
272            break;
273       }
274
275       /*update old-new data*/
276       /* just swap pointers instead of a long copy */
277       {
278          spx_word16_t **tmp2;
279          tmp2=ot;
280          ot=nt;
281          nt=tmp2;
282       }
283       for (j=0;j<N;j++)
284          for (m=0;m<nb_subvect;m++)
285             oind[j][m]=nind[j][m];
286       for (j=0;j<N;j++)
287          odist[j]=ndist[j];
288    }
289
290    /*save indices*/
291    for (i=0;i<nb_subvect;i++)
292    {
293       ind[i]=nind[0][i];
294       speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
295    }
296    
297    /* Put everything back together */
298    for (i=0;i<nb_subvect;i++)
299    {
300       int rind;
301       spx_word16_t sign=1;
302       rind = ind[i];
303       if (rind>=shape_cb_size)
304       {
305          sign=-1;
306          rind-=shape_cb_size;
307       }
308 #ifdef FIXED_POINT
309       if (sign==1)
310       {
311          for (j=0;j<subvect_size;j++)
312             e[subvect_size*i+j]=SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
313       } else {
314          for (j=0;j<subvect_size;j++)
315             e[subvect_size*i+j]=-SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
316       }
317 #else
318       for (j=0;j<subvect_size;j++)
319          e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
320 #endif
321    }   
322    /* Update excitation */
323    for (j=0;j<nsf;j++)
324       exc[j]+=e[j];
325    
326    /* Update target */
327    syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
328    for (j=0;j<nsf;j++)
329       target[j]-=r2[j];
330
331 }
332
333
334 void split_cb_shape_sign_unquant(
335 spx_sig_t *exc,
336 const void *par,                      /* non-overlapping codebook */
337 int   nsf,                      /* number of samples in subframe */
338 SpeexBits *bits,
339 char *stack
340 )
341 {
342    int i,j;
343    int *ind, *signs;
344    const signed char *shape_cb;
345    int shape_cb_size, subvect_size, nb_subvect;
346    split_cb_params *params;
347    int have_sign;
348
349    params = (split_cb_params *) par;
350    subvect_size = params->subvect_size;
351    nb_subvect = params->nb_subvect;
352    shape_cb_size = 1<<params->shape_bits;
353    shape_cb = params->shape_cb;
354    have_sign = params->have_sign;
355
356    ind = PUSH(stack, nb_subvect, int);
357    signs = PUSH(stack, nb_subvect, int);
358
359    /* Decode codewords and gains */
360    for (i=0;i<nb_subvect;i++)
361    {
362       if (have_sign)
363          signs[i] = speex_bits_unpack_unsigned(bits, 1);
364       else
365          signs[i] = 0;
366       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
367    }
368    /* Compute decoded excitation */
369    for (i=0;i<nb_subvect;i++)
370    {
371       spx_word16_t s=1;
372       if (signs[i])
373          s=-1;
374 #ifdef FIXED_POINT
375       if (s==1)
376       {
377          for (j=0;j<subvect_size;j++)
378             exc[subvect_size*i+j]=SHL((spx_word32_t)shape_cb[ind[i]*subvect_size+j],SIG_SHIFT-5);
379       } else {
380          for (j=0;j<subvect_size;j++)
381             exc[subvect_size*i+j]=-SHL((spx_word32_t)shape_cb[ind[i]*subvect_size+j],SIG_SHIFT-5);
382       }
383 #else
384       for (j=0;j<subvect_size;j++)
385          exc[subvect_size*i+j]+=s*0.03125*shape_cb[ind[i]*subvect_size+j];      
386 #endif
387    }
388 }
389
390 void noise_codebook_quant(
391 spx_sig_t target[],                     /* target vector */
392 spx_coef_t ak[],                        /* LPCs for this subframe */
393 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
394 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
395 const void *par,                      /* Codebook/search parameters*/
396 int   p,                        /* number of LPC coeffs */
397 int   nsf,                      /* number of samples in subframe */
398 spx_sig_t *exc,
399 spx_sig_t *r,
400 SpeexBits *bits,
401 char *stack,
402 int   complexity
403 )
404 {
405    int i;
406    spx_sig_t *tmp=PUSH(stack, nsf, spx_sig_t);
407    residue_percep_zero(target, ak, awk1, awk2, tmp, nsf, p, stack);
408
409    for (i=0;i<nsf;i++)
410       exc[i]+=tmp[i];
411    for (i=0;i<nsf;i++)
412       target[i]=0;
413
414 }
415
416
417 void noise_codebook_unquant(
418 spx_sig_t *exc,
419 const void *par,                      /* non-overlapping codebook */
420 int   nsf,                      /* number of samples in subframe */
421 SpeexBits *bits,
422 char *stack
423 )
424 {
425    speex_rand_vec(1, exc, nsf);
426 }