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