500352bbf48320801bacadfcd91cbd21c7afc107
[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 #elif defined(ARM4_ASM) || defined(ARM5E_ASM)
46 #include "cb_search_arm4.h"
47 #else
48
49 static void compute_weighted_codebook(const signed char *shape_cb, const spx_sig_t *r, spx_word16_t *resp, spx_word16_t *resp2, spx_word32_t *E, int shape_cb_size, int subvect_size, char *stack)
50 {
51    int i, j, k;
52    for (i=0;i<shape_cb_size;i++)
53    {
54       spx_word16_t *res;
55       const signed char *shape;
56
57       res = resp+i*subvect_size;
58       shape = shape_cb+i*subvect_size;
59       E[i]=0;
60
61       /* Compute codeword response using convolution with impulse response */
62       for(j=0;j<subvect_size;j++)
63       {
64          spx_word32_t resj=0;
65          for (k=0;k<=j;k++)
66             resj = MAC16_16(resj,shape[k],r[j-k]);
67 #ifdef FIXED_POINT
68          resj = SHR(resj, 11);
69 #else
70          resj *= 0.03125;
71 #endif
72          /* Compute codeword energy */
73          E[i]=ADD32(E[i],MULT16_16(resj,resj));
74          res[j] = resj;
75          /*printf ("%d\n", (int)res[j]);*/
76       }
77    }
78
79 }
80
81 #endif
82
83
84
85 #if 0
86
87 void split_cb_search_shape_sign(
88 spx_sig_t target[],                     /* target vector */
89 spx_coef_t ak[],                        /* LPCs for this subframe */
90 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
91 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
92 const void *par,                      /* Codebook/search parameters*/
93 int   p,                        /* number of LPC coeffs */
94 int   nsf,                      /* number of samples in subframe */
95 spx_sig_t *exc,
96 spx_sig_t *r,
97 SpeexBits *bits,
98 char *stack,
99 int   complexity,
100 int   update_target
101                                )
102 {
103    int i,j,k,m,n,q;
104    spx_word16_t *resp;
105 #ifdef _USE_SSE
106    __m128 *resp2;
107    __m128 *E;
108 #else
109    spx_word16_t *resp2;
110    spx_word32_t *E;
111 #endif
112    spx_word16_t *t;
113    spx_sig_t *e, *r2;
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    if (N<1)
125       N=1;
126    
127    params = (split_cb_params *) par;
128    subvect_size = params->subvect_size;
129    nb_subvect = params->nb_subvect;
130    shape_cb_size = 1<<params->shape_bits;
131    shape_cb = params->shape_cb;
132    have_sign = params->have_sign;
133    resp = PUSH(stack, shape_cb_size*subvect_size, spx_word16_t);
134 #ifdef _USE_SSE
135    resp2 = PUSH(stack, (shape_cb_size*subvect_size)>>2, __m128);
136    E = PUSH(stack, shape_cb_size>>2, __m128);
137 #else
138    resp2 = resp;
139    E = PUSH(stack, shape_cb_size, spx_word32_t);
140 #endif
141    t = PUSH(stack, nsf, spx_word16_t);
142    e = PUSH(stack, nsf, spx_sig_t);
143    r2 = PUSH(stack, nsf, spx_sig_t);
144    
145    /* FIXME: make that adaptive? */
146    for (i=0;i<nsf;i++)
147       t[i]=SHR(target[i],6);
148
149    compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
150
151    for (i=0;i<nb_subvect;i++)
152    {
153       spx_word16_t *x=t+subvect_size*i;
154       /*Find new n-best based on previous n-best j*/
155       if (have_sign)
156          vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
157       else
158          vq_nbest(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
159       
160       speex_bits_pack(bits,best_index,params->shape_bits+have_sign);
161       
162       /* New code: update only enough of the target to calculate error*/
163       {
164          int rind;
165          spx_word16_t *res;
166          spx_word16_t sign=1;
167          rind = best_index;
168          if (rind>=shape_cb_size)
169          {
170             sign=-1;
171             rind-=shape_cb_size;
172          }
173          res = resp+rind*subvect_size;
174          if (sign>0)
175             for (m=0;m<subvect_size;m++)
176                t[subvect_size*i+m] -= res[m];
177          else
178             for (m=0;m<subvect_size;m++)
179                t[subvect_size*i+m] += res[m];
180
181 #ifdef FIXED_POINT
182          if (sign)
183          {
184             for (j=0;j<subvect_size;j++)
185                e[subvect_size*i+j]=SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
186          } else {
187             for (j=0;j<subvect_size;j++)
188                e[subvect_size*i+j]=-SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
189          }
190 #else
191          for (j=0;j<subvect_size;j++)
192             e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
193 #endif
194       
195       }
196             
197       for (m=0;m<subvect_size;m++)
198       {
199          spx_word16_t g;
200          int rind;
201          spx_word16_t sign=1;
202          rind = best_index;
203          if (rind>=shape_cb_size)
204          {
205             sign=-1;
206             rind-=shape_cb_size;
207          }
208          
209          q=subvect_size-m;
210 #ifdef FIXED_POINT
211          g=sign*shape_cb[rind*subvect_size+m];
212          for (n=subvect_size*(i+1);n<nsf;n++,q++)
213             t[n] = SUB32(t[n],MULT16_16_Q11(g,r[q]));
214 #else
215          g=sign*0.03125*shape_cb[rind*subvect_size+m];
216          for (n=subvect_size*(i+1);n<nsf;n++,q++)
217             t[n] = SUB32(t[n],g*r[q]);
218 #endif
219       }
220
221
222    }
223
224    /* Update excitation */
225    for (j=0;j<nsf;j++)
226       exc[j]+=e[j];
227    
228    /* Update target: only update target if necessary */
229    if (update_target)
230    {
231       syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
232       for (j=0;j<nsf;j++)
233          target[j]-=r2[j];
234    }
235 }
236
237 #else
238
239 void split_cb_search_shape_sign(
240 spx_sig_t target[],                     /* target vector */
241 spx_coef_t ak[],                        /* LPCs for this subframe */
242 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
243 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
244 const void *par,                      /* Codebook/search parameters*/
245 int   p,                        /* number of LPC coeffs */
246 int   nsf,                      /* number of samples in subframe */
247 spx_sig_t *exc,
248 spx_sig_t *r,
249 SpeexBits *bits,
250 char *stack,
251 int   complexity,
252 int   update_target
253 )
254 {
255    int i,j,k,m,n,q;
256    spx_word16_t *resp;
257 #ifdef _USE_SSE
258    __m128 *resp2;
259    __m128 *E;
260 #else
261    spx_word16_t *resp2;
262    spx_word32_t *E;
263 #endif
264    spx_word16_t *t;
265    spx_sig_t *e, *r2;
266    spx_word16_t *tmp;
267    spx_word32_t *ndist, *odist;
268    int *itmp;
269    spx_word16_t **ot, **nt;
270    int **nind, **oind;
271    int *ind;
272    const signed char *shape_cb;
273    int shape_cb_size, subvect_size, nb_subvect;
274    split_cb_params *params;
275    int N=2;
276    int *best_index;
277    spx_word32_t *best_dist;
278    int have_sign;
279    N=complexity;
280    if (N>10)
281       N=10;
282    if (N<1)
283       N=1;
284    
285    ot=PUSH(stack, N, spx_word16_t*);
286    nt=PUSH(stack, N, spx_word16_t*);
287    oind=PUSH(stack, N, int*);
288    nind=PUSH(stack, N, int*);
289
290    params = (split_cb_params *) par;
291    subvect_size = params->subvect_size;
292    nb_subvect = params->nb_subvect;
293    shape_cb_size = 1<<params->shape_bits;
294    shape_cb = params->shape_cb;
295    have_sign = params->have_sign;
296    resp = PUSH(stack, shape_cb_size*subvect_size, spx_word16_t);
297 #ifdef _USE_SSE
298    resp2 = PUSH(stack, (shape_cb_size*subvect_size)>>2, __m128);
299    E = PUSH(stack, shape_cb_size>>2, __m128);
300 #else
301    resp2 = resp;
302    E = PUSH(stack, shape_cb_size, spx_word32_t);
303 #endif
304    t = PUSH(stack, nsf, spx_word16_t);
305    e = PUSH(stack, nsf, spx_sig_t);
306    r2 = PUSH(stack, nsf, spx_sig_t);
307    ind = PUSH(stack, nb_subvect, int);
308
309    tmp = PUSH(stack, 2*N*nsf, spx_word16_t);
310    for (i=0;i<N;i++)
311    {
312       ot[i]=tmp;
313       tmp += nsf;
314       nt[i]=tmp;
315       tmp += nsf;
316    }
317    best_index = PUSH(stack, N, int);
318    best_dist = PUSH(stack, N, spx_word32_t);
319    ndist = PUSH(stack, N, spx_word32_t);
320    odist = PUSH(stack, N, spx_word32_t);
321    
322    itmp = PUSH(stack, 2*N*nb_subvect, int);
323    for (i=0;i<N;i++)
324    {
325       nind[i]=itmp;
326       itmp+=nb_subvect;
327       oind[i]=itmp;
328       itmp+=nb_subvect;
329       for (j=0;j<nb_subvect;j++)
330          nind[i][j]=oind[i][j]=-1;
331    }
332    
333    /* FIXME: make that adaptive? */
334    for (i=0;i<nsf;i++)
335       t[i]=SHR(target[i],6);
336
337    for (j=0;j<N;j++)
338       for (i=0;i<nsf;i++)
339          ot[j][i]=t[i];
340
341    /*for (i=0;i<nsf;i++)
342      printf ("%d\n", (int)t[i]);*/
343
344    /* Pre-compute codewords response and energy */
345    compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
346
347    for (j=0;j<N;j++)
348       odist[j]=0;
349    /*For all subvectors*/
350    for (i=0;i<nb_subvect;i++)
351    {
352       /*"erase" nbest list*/
353       for (j=0;j<N;j++)
354          ndist[j]=-2;
355
356       /*For all n-bests of previous subvector*/
357       for (j=0;j<N;j++)
358       {
359          spx_word16_t *x=ot[j]+subvect_size*i;
360          /*Find new n-best based on previous n-best j*/
361          if (have_sign)
362             vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
363          else
364             vq_nbest(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
365
366          /*For all new n-bests*/
367          for (k=0;k<N;k++)
368          {
369             spx_word16_t *ct;
370             spx_word32_t err=0;
371             ct = ot[j];
372             /*update target*/
373
374             /*previous target*/
375             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
376                t[m]=ct[m];
377
378             /* New code: update only enough of the target to calculate error*/
379             {
380                int rind;
381                spx_word16_t *res;
382                spx_word16_t sign=1;
383                rind = best_index[k];
384                if (rind>=shape_cb_size)
385                {
386                   sign=-1;
387                   rind-=shape_cb_size;
388                }
389                res = resp+rind*subvect_size;
390                if (sign>0)
391                   for (m=0;m<subvect_size;m++)
392                      t[subvect_size*i+m] -= res[m];
393                else
394                   for (m=0;m<subvect_size;m++)
395                      t[subvect_size*i+m] += res[m];
396             }
397             
398             /*compute error (distance)*/
399             err=odist[j];
400             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
401                err = MAC16_16(err, t[m],t[m]);
402             /*update n-best list*/
403             if (err<ndist[N-1] || ndist[N-1]<-1)
404             {
405
406                /*previous target (we don't care what happened before*/
407                for (m=(i+1)*subvect_size;m<nsf;m++)
408                   t[m]=ct[m];
409                /* New code: update the rest of the target only if it's worth it */
410                for (m=0;m<subvect_size;m++)
411                {
412                   spx_word16_t g;
413                   int rind;
414                   spx_word16_t sign=1;
415                   rind = best_index[k];
416                   if (rind>=shape_cb_size)
417                   {
418                      sign=-1;
419                      rind-=shape_cb_size;
420                   }
421
422                   q=subvect_size-m;
423 #ifdef FIXED_POINT
424                   g=sign*shape_cb[rind*subvect_size+m];
425                   for (n=subvect_size*(i+1);n<nsf;n++,q++)
426                      t[n] = SUB32(t[n],MULT16_16_Q11(g,r[q]));
427 #else
428                   g=sign*0.03125*shape_cb[rind*subvect_size+m];
429                   for (n=subvect_size*(i+1);n<nsf;n++,q++)
430                      t[n] = SUB32(t[n],g*r[q]);
431 #endif
432                }
433
434
435                for (m=0;m<N;m++)
436                {
437                   if (err < ndist[m] || ndist[m]<-1)
438                   {
439                      for (n=N-1;n>m;n--)
440                      {
441                         for (q=(i+1)*subvect_size;q<nsf;q++)
442                            nt[n][q]=nt[n-1][q];
443                         for (q=0;q<nb_subvect;q++)
444                            nind[n][q]=nind[n-1][q];
445                         ndist[n]=ndist[n-1];
446                      }
447                      for (q=(i+1)*subvect_size;q<nsf;q++)
448                         nt[m][q]=t[q];
449                      for (q=0;q<nb_subvect;q++)
450                         nind[m][q]=oind[j][q];
451                      nind[m][i]=best_index[k];
452                      ndist[m]=err;
453                      break;
454                   }
455                }
456             }
457          }
458          if (i==0)
459            break;
460       }
461
462       /*update old-new data*/
463       /* just swap pointers instead of a long copy */
464       {
465          spx_word16_t **tmp2;
466          tmp2=ot;
467          ot=nt;
468          nt=tmp2;
469       }
470       for (j=0;j<N;j++)
471          for (m=0;m<nb_subvect;m++)
472             oind[j][m]=nind[j][m];
473       for (j=0;j<N;j++)
474          odist[j]=ndist[j];
475    }
476
477    /*save indices*/
478    for (i=0;i<nb_subvect;i++)
479    {
480       ind[i]=nind[0][i];
481       speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
482    }
483    
484    /* Put everything back together */
485    for (i=0;i<nb_subvect;i++)
486    {
487       int rind;
488       spx_word16_t sign=1;
489       rind = ind[i];
490       if (rind>=shape_cb_size)
491       {
492          sign=-1;
493          rind-=shape_cb_size;
494       }
495 #ifdef FIXED_POINT
496       if (sign==1)
497       {
498          for (j=0;j<subvect_size;j++)
499             e[subvect_size*i+j]=SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
500       } else {
501          for (j=0;j<subvect_size;j++)
502             e[subvect_size*i+j]=-SHL((spx_word32_t)shape_cb[rind*subvect_size+j],SIG_SHIFT-5);
503       }
504 #else
505       for (j=0;j<subvect_size;j++)
506          e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
507 #endif
508    }   
509    /* Update excitation */
510    for (j=0;j<nsf;j++)
511       exc[j]+=e[j];
512    
513    /* Update target: only update target if necessary */
514    if (update_target)
515    {
516       syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
517       for (j=0;j<nsf;j++)
518          target[j]-=r2[j];
519    }
520 }
521 #endif
522
523 void split_cb_shape_sign_unquant(
524 spx_sig_t *exc,
525 const void *par,                      /* non-overlapping codebook */
526 int   nsf,                      /* number of samples in subframe */
527 SpeexBits *bits,
528 char *stack
529 )
530 {
531    int i,j;
532    int *ind, *signs;
533    const signed char *shape_cb;
534    int shape_cb_size, subvect_size, nb_subvect;
535    split_cb_params *params;
536    int have_sign;
537
538    params = (split_cb_params *) par;
539    subvect_size = params->subvect_size;
540    nb_subvect = params->nb_subvect;
541    shape_cb_size = 1<<params->shape_bits;
542    shape_cb = params->shape_cb;
543    have_sign = params->have_sign;
544
545    ind = PUSH(stack, nb_subvect, int);
546    signs = PUSH(stack, nb_subvect, int);
547
548    /* Decode codewords and gains */
549    for (i=0;i<nb_subvect;i++)
550    {
551       if (have_sign)
552          signs[i] = speex_bits_unpack_unsigned(bits, 1);
553       else
554          signs[i] = 0;
555       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
556    }
557    /* Compute decoded excitation */
558    for (i=0;i<nb_subvect;i++)
559    {
560       spx_word16_t s=1;
561       if (signs[i])
562          s=-1;
563 #ifdef FIXED_POINT
564       if (s==1)
565       {
566          for (j=0;j<subvect_size;j++)
567             exc[subvect_size*i+j]=SHL((spx_word32_t)shape_cb[ind[i]*subvect_size+j],SIG_SHIFT-5);
568       } else {
569          for (j=0;j<subvect_size;j++)
570             exc[subvect_size*i+j]=-SHL((spx_word32_t)shape_cb[ind[i]*subvect_size+j],SIG_SHIFT-5);
571       }
572 #else
573       for (j=0;j<subvect_size;j++)
574          exc[subvect_size*i+j]+=s*0.03125*shape_cb[ind[i]*subvect_size+j];      
575 #endif
576    }
577 }
578
579 void noise_codebook_quant(
580 spx_sig_t target[],                     /* target vector */
581 spx_coef_t ak[],                        /* LPCs for this subframe */
582 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
583 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
584 const void *par,                      /* Codebook/search parameters*/
585 int   p,                        /* number of LPC coeffs */
586 int   nsf,                      /* number of samples in subframe */
587 spx_sig_t *exc,
588 spx_sig_t *r,
589 SpeexBits *bits,
590 char *stack,
591 int   complexity,
592 int   update_target
593 )
594 {
595    int i;
596    spx_sig_t *tmp=PUSH(stack, nsf, spx_sig_t);
597    residue_percep_zero(target, ak, awk1, awk2, tmp, nsf, p, stack);
598
599    for (i=0;i<nsf;i++)
600       exc[i]+=tmp[i];
601    for (i=0;i<nsf;i++)
602       target[i]=0;
603
604 }
605
606
607 void noise_codebook_unquant(
608 spx_sig_t *exc,
609 const void *par,                      /* non-overlapping codebook */
610 int   nsf,                      /* number of samples in subframe */
611 SpeexBits *bits,
612 char *stack
613 )
614 {
615    speex_rand_vec(1, exc, nsf);
616 }