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