Win32 support, thanks to john33 (Hydrogen Audio)
[speexdsp.git] / libspeex / cb_search.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: cb_search.c
3
4    This library is free software; you can redistribute it and/or
5    modify it under the terms of the GNU Lesser General Public
6    License as published by the Free Software Foundation; either
7    version 2.1 of the License, or (at your option) any later version.
8    
9    This library is distributed in the hope that it will be useful,
10    but WITHOUT ANY WARRANTY; without even the implied warranty of
11    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
12    Lesser General Public License for more details.
13    
14    You should have received a copy of the GNU Lesser General Public
15    License along with this library; if not, write to the Free Software
16    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17 */
18
19
20
21 #include <stdlib.h>
22 #include "cb_search.h"
23 #include "filters.h"
24 #include <math.h>
25 #ifdef DEBUG
26 #include <stdio.h>
27 #endif
28 #include "stack_alloc.h"
29 #include "vq.h"
30
31 #ifndef min
32 # define min(a,b) ((a) < (b) ? (a) : (b))
33 #endif
34 #ifndef max
35 # define max(a,b) ((a) > (b) ? (a) : (b))
36 #endif
37
38
39 void split_cb_search_nogain(
40 float target[],                 /* target vector */
41 float ak[],                     /* LPCs for this subframe */
42 float awk1[],                   /* Weighted LPCs for this subframe */
43 float awk2[],                   /* Weighted LPCs for this subframe */
44 void *par,                      /* Codebook/search parameters*/
45 int   p,                        /* number of LPC coeffs */
46 int   nsf,                      /* number of samples in subframe */
47 float *exc,
48 SpeexBits *bits,
49 float *stack,
50 int   complexity
51 )
52 {
53    int i,j,k,m,n,q;
54    float *resp;
55    float *t, *r, *e, *E;
56    /*FIXME: Should make this dynamic*/
57    float *tmp, *ot[20], *nt[20];
58    float *ndist, *odist;
59    int *itmp, *nind[20], *oind[20];
60
61    int *ind;
62    float *shape_cb;
63    int shape_cb_size, subvect_size, nb_subvect;
64    split_cb_params *params;
65    int N=2;
66    int *best_index;
67    float *best_dist;
68
69    N=complexity;
70    if (N<1)
71       N=1;
72    if (N>10)
73       N=10;
74
75    params = (split_cb_params *) par;
76    subvect_size = params->subvect_size;
77    nb_subvect = params->nb_subvect;
78    shape_cb_size = 1<<params->shape_bits;
79    shape_cb = params->shape_cb;
80    resp = PUSH(stack, shape_cb_size*subvect_size);
81    t = PUSH(stack, nsf);
82    r = PUSH(stack, nsf);
83    e = PUSH(stack, nsf);
84    E = PUSH(stack, shape_cb_size);
85    ind = (int*)PUSH(stack, nb_subvect);
86
87    tmp = PUSH(stack, 2*N*nsf);
88    for (i=0;i<N;i++)
89    {
90       ot[i]=tmp;
91       tmp += nsf;
92       nt[i]=tmp;
93       tmp += nsf;
94    }
95
96    best_index = (int*)PUSH(stack, N);
97    best_dist = PUSH(stack, N);
98    ndist = PUSH(stack, N);
99    odist = PUSH(stack, N);
100    
101    itmp = (int*)PUSH(stack, 2*N*nb_subvect);
102    for (i=0;i<N;i++)
103    {
104       nind[i]=itmp;
105       itmp+=nb_subvect;
106       oind[i]=itmp;
107       itmp+=nb_subvect;
108       for (j=0;j<nb_subvect;j++)
109          nind[i][j]=oind[i][j]=-1;
110    }
111
112    for (j=0;j<N;j++)
113       for (i=0;i<nsf;i++)
114          ot[j][i]=target[i];
115
116    for (i=0;i<nsf;i++)
117       t[i]=target[i];
118
119    e[0]=1;
120    for (i=1;i<nsf;i++)
121       e[i]=0;
122    residue_zero(e, awk1, r, nsf, p);
123    syn_filt_zero(r, ak, r, nsf, p);
124    syn_filt_zero(r, awk2, r, nsf,p);
125    
126    /* Pre-compute codewords response and energy */
127    for (i=0;i<shape_cb_size;i++)
128    {
129       float *res = resp+i*subvect_size;
130
131       /* Compute codeword response */
132       int k;
133       for(j=0;j<subvect_size;j++)
134          res[j]=0;
135       for(j=0;j<subvect_size;j++)
136       {
137          for (k=j;k<subvect_size;k++)
138             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
139       }
140       E[i]=0;
141       for(j=0;j<subvect_size;j++)
142          E[i]+=res[j]*res[j];
143    }
144
145    for (j=0;j<N;j++)
146       odist[j]=0;
147    /*For all subvectors*/
148    for (i=0;i<nb_subvect;i++)
149    {
150       /*"erase" nbest list*/
151       for (j=0;j<N;j++)
152          ndist[j]=-1;
153
154       /*For all n-bests of previous subvector*/
155       for (j=0;j<N;j++)
156       {
157          float *x=ot[j]+subvect_size*i;
158          /*Find new n-best based on previous n-best j*/
159          vq_nbest(x, resp, subvect_size, shape_cb_size, E, N, best_index, best_dist);
160
161          /*For all new n-bests*/
162          for (k=0;k<N;k++)
163          {
164             float err=0;
165             /*previous target*/
166             for (m=0;m<nsf;m++)
167                t[m]=ot[j][m];
168             /*update target*/
169             for (m=0;m<subvect_size;m++)
170             {
171                float g=shape_cb[best_index[k]*subvect_size+m];
172                for (n=subvect_size*i+m,q=0;n<nsf;n++,q++)
173                   t[n] -= g*r[q];
174             }
175             
176             /*compute error (distance)*/
177             err=odist[j];
178             for (m=i*subvect_size;m<(i+1)*subvect_size;m++)
179                err += t[m]*t[m];
180             /*update n-best list*/
181             if (err<ndist[N-1] || ndist[N-1]<-.5)
182             {
183                for (m=0;m<N;m++)
184                {
185                   if (err < ndist[m] || ndist[m]<-.5)
186                   {
187                      for (n=N-1;n>m;n--)
188                      {
189                         for (q=0;q<nsf;q++)
190                            nt[n][q]=nt[n-1][q];
191                         for (q=0;q<nb_subvect;q++)
192                            nind[n][q]=nind[n-1][q];
193                         ndist[n]=ndist[n-1];
194                      }
195                      for (q=0;q<nsf;q++)
196                         nt[m][q]=t[q];
197                      for (q=0;q<nb_subvect;q++)
198                         nind[m][q]=oind[j][q];
199                      nind[m][i]=best_index[k];
200                      ndist[m]=err;
201                      break;
202                   }
203                }
204             }
205          }
206          if (i==0)
207            break;
208       }
209
210       /*opdate old-new data*/
211       for (j=0;j<N;j++)
212          for (m=0;m<nsf;m++)
213             ot[j][m]=nt[j][m];
214       for (j=0;j<N;j++)
215          for (m=0;m<nb_subvect;m++)
216             oind[j][m]=nind[j][m];
217       for (j=0;j<N;j++)
218          odist[j]=ndist[j];
219    }
220
221    /*save indices*/
222    for (i=0;i<nb_subvect;i++)
223    {
224       ind[i]=nind[0][i];
225       speex_bits_pack(bits,ind[i],params->shape_bits);
226    }
227    
228    /* Put everything back together */
229    for (i=0;i<nb_subvect;i++)
230       for (j=0;j<subvect_size;j++)
231          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
232
233    /* Update excitation */
234    for (j=0;j<nsf;j++)
235       exc[j]+=e[j];
236    
237    /* Update target */
238    residue_zero(e, awk1, r, nsf, p);
239    syn_filt_zero(r, ak, r, nsf, p);
240    syn_filt_zero(r, awk2, r, nsf,p);
241    for (j=0;j<nsf;j++)
242       target[j]-=r[j];
243
244    POP(stack);
245    POP(stack);
246    POP(stack);
247    POP(stack);
248    POP(stack);
249    POP(stack);
250    POP(stack);
251    POP(stack);
252    POP(stack);
253    POP(stack);
254    POP(stack);
255    POP(stack);
256 }
257
258
259
260
261
262 void split_cb_search_shape_sign(
263 float target[],                 /* target vector */
264 float ak[],                     /* LPCs for this subframe */
265 float awk1[],                   /* Weighted LPCs for this subframe */
266 float awk2[],                   /* Weighted LPCs for this subframe */
267 void *par,                      /* Codebook/search parameters*/
268 int   p,                        /* number of LPC coeffs */
269 int   nsf,                      /* number of samples in subframe */
270 float *exc,
271 SpeexBits *bits,
272 float *stack,
273 int   complexity
274 )
275 {
276    int i,j;
277    float *resp;
278    float *t, *r, *e, *E;
279    int *ind, *signs;
280    float *shape_cb;
281    int shape_cb_size, subvect_size, nb_subvect;
282    split_cb_params *params;
283
284    params = (split_cb_params *) par;
285    subvect_size = params->subvect_size;
286    nb_subvect = params->nb_subvect;
287    shape_cb_size = 1<<params->shape_bits;
288    shape_cb = params->shape_cb;
289    resp = PUSH(stack, shape_cb_size*subvect_size);
290    t = PUSH(stack, nsf);
291    r = PUSH(stack, nsf);
292    e = PUSH(stack, nsf);
293    E = PUSH(stack, shape_cb_size);
294    ind = (int*)PUSH(stack, nb_subvect);
295    signs = (int*)PUSH(stack, nb_subvect);
296
297    for (i=0;i<nsf;i++)
298       t[i]=target[i];
299
300    e[0]=1;
301    for (i=1;i<nsf;i++)
302       e[i]=0;
303    residue_zero(e, awk1, r, nsf, p);
304    syn_filt_zero(r, ak, r, nsf, p);
305    syn_filt_zero(r, awk2, r, nsf,p);
306    
307    /* Pre-compute codewords response and energy */
308    for (i=0;i<shape_cb_size;i++)
309    {
310       float *res = resp+i*subvect_size;
311
312       /* Compute codeword response */
313       int k;
314       for(j=0;j<subvect_size;j++)
315          res[j]=0;
316       for(j=0;j<subvect_size;j++)
317       {
318          for (k=j;k<subvect_size;k++)
319             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
320       }
321       E[i]=0;
322       for(j=0;j<subvect_size;j++)
323          E[i]+=res[j]*res[j];
324    }
325
326    for (i=0;i<nb_subvect;i++)
327    {
328       int best_index[2]={0,0}, k, m;
329       float g, dist, best_dist[2]={-1,-1}, best_sign[2]={0,0};
330       float *a, *x;
331       float energy=0;
332       x=t+subvect_size*i;
333
334       for (k=0;k<subvect_size;k++)
335          energy+=x[k]*x[k];
336       /* Find best codeword for current sub-vector */
337       for (j=0;j<shape_cb_size;j++)
338       {
339          int sign;
340          dist=0;
341          a=resp+j*subvect_size;
342          dist=0;
343          for (k=0;k<subvect_size;k++)
344             dist -= 2*a[k]*x[k];
345          if (dist > 0)
346          {
347             sign=1;
348             dist =- dist;
349          } else
350             sign=0;
351          dist += energy+E[j];
352          if (dist<best_dist[0] || best_dist[0]<0)
353          {
354             best_dist[1]=best_dist[0];
355             best_index[1]=best_index[0];
356             best_sign[1]=best_sign[0];
357             best_dist[0]=dist;
358             best_index[0]=j;
359             best_sign[0]=sign;
360          } else if (dist<best_dist[1] || best_dist[1]<0)
361          {
362             best_dist[1]=dist;
363             best_index[1]=j;
364             best_sign[1]=sign;
365          }
366       }
367       if (i<nb_subvect-1)
368       {
369          int nbest;
370          float *tt, err[2];
371          float best_score[2];
372          tt=PUSH(stack,nsf);
373          for (nbest=0;nbest<2;nbest++)
374          {
375             float s=1;
376             if (best_sign[nbest])
377                s=-1;
378             for (j=0;j<nsf;j++)
379                tt[j]=t[j];
380             for (j=0;j<subvect_size;j++)
381             {
382                g=s*shape_cb[best_index[nbest]*subvect_size+j];
383                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
384                   tt[k] -= g*r[m];
385             }
386             
387             {
388                int best_index2=0, best_sign2=0, sign2;
389                float  best_dist2=0;
390                x=t+subvect_size*(i+1);
391                for (j=0;j<shape_cb_size;j++)
392                {
393                   a=resp+j*subvect_size;
394                   dist = 0;
395                   for (k=0;k<subvect_size;k++)
396                      dist -= 2*a[k]*x[k];
397                   if (dist > 0)
398                   {
399                      sign2=1;
400                      dist =- dist;
401                   } else
402                      sign2=0;
403                   dist += energy+E[j];
404                   if (dist<best_dist2 || j==0)
405                   {
406                      best_dist2=dist;
407                      best_index2=j;
408                      best_sign2=sign2;
409                   }
410                }
411                s=1;
412                if (best_sign2)
413                   s=-1;
414                /*int i2=vq_index(&tt[subvect_size*(i+1)], resp, subvect_size, shape_cb_size);*/
415                
416                for (j=0;j<subvect_size;j++)
417                {
418                   g=s*shape_cb[best_index2*subvect_size+j];
419                   for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
420                      tt[k] -= g*r[m];
421                }
422             }
423
424             err[nbest]=0;
425             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
426                err[nbest]-=tt[j]*tt[j];
427             
428             best_score[nbest]=err[nbest];
429          }
430
431          if (best_score[1]>best_score[0])
432          {
433             best_sign[0]=best_sign[1];
434             best_index[0]=best_index[1];
435             best_score[0]=best_score[1];
436          }
437          POP(stack);
438
439       }
440
441       ind[i]=best_index[0];
442       signs[i] = best_sign[0];
443
444       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
445       speex_bits_pack(bits,signs[i],1);
446       speex_bits_pack(bits,ind[i],params->shape_bits);
447
448       /* Update target for next subvector */
449       for (j=0;j<subvect_size;j++)
450       {
451          g=shape_cb[ind[i]*subvect_size+j];
452          if (signs[i])
453             g=-g;
454          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
455             t[k] -= g*r[m];
456       }
457    }
458    
459    /* Put everything back together */
460    for (i=0;i<nb_subvect;i++)
461    {
462       float s=1;
463       if (signs[i])
464          s=-1;
465       for (j=0;j<subvect_size;j++)
466          e[subvect_size*i+j]=s*shape_cb[ind[i]*subvect_size+j];
467    }
468    /* Update excitation */
469    for (j=0;j<nsf;j++)
470       exc[j]+=e[j];
471    
472    /* Update target */
473    residue_zero(e, awk1, r, nsf, p);
474    syn_filt_zero(r, ak, r, nsf, p);
475    syn_filt_zero(r, awk2, r, nsf,p);
476    for (j=0;j<nsf;j++)
477       target[j]-=r[j];
478
479    
480    POP(stack);
481    POP(stack);
482    POP(stack);
483    POP(stack);
484    POP(stack);
485    POP(stack);
486    POP(stack);
487 }
488
489
490 void split_cb_nogain_unquant(
491 float *exc,
492 void *par,                      /* non-overlapping codebook */
493 int   nsf,                      /* number of samples in subframe */
494 SpeexBits *bits,
495 float *stack
496 )
497 {
498    int i,j;
499    int *ind;
500    float *shape_cb;
501    int shape_cb_size, subvect_size, nb_subvect;
502    split_cb_params *params;
503
504    params = (split_cb_params *) par;
505    subvect_size = params->subvect_size;
506    nb_subvect = params->nb_subvect;
507    shape_cb_size = 1<<params->shape_bits;
508    shape_cb = params->shape_cb;
509    
510    ind = (int*)PUSH(stack, nb_subvect);
511
512    /* Decode codewords and gains */
513    for (i=0;i<nb_subvect;i++)
514    {
515       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
516    }
517    /* Compute decoded excitation */
518    for (i=0;i<nb_subvect;i++)
519       for (j=0;j<subvect_size;j++)
520          exc[subvect_size*i+j]+=shape_cb[ind[i]*subvect_size+j];
521
522    POP(stack);
523 }
524
525 void split_cb_shape_sign_unquant(
526 float *exc,
527 void *par,                      /* non-overlapping codebook */
528 int   nsf,                      /* number of samples in subframe */
529 SpeexBits *bits,
530 float *stack
531 )
532 {
533    int i,j;
534    int *ind, *signs;
535    float *shape_cb;
536    int shape_cb_size, subvect_size, nb_subvect;
537    split_cb_params *params;
538
539    params = (split_cb_params *) par;
540    subvect_size = params->subvect_size;
541    nb_subvect = params->nb_subvect;
542    shape_cb_size = 1<<params->shape_bits;
543    shape_cb = params->shape_cb;
544    
545    ind = (int*)PUSH(stack, nb_subvect);
546    signs = (int*)PUSH(stack, nb_subvect);
547
548    /* Decode codewords and gains */
549    for (i=0;i<nb_subvect;i++)
550    {
551       signs[i] = speex_bits_unpack_unsigned(bits, 1);
552       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
553    }
554    /* Compute decoded excitation */
555    for (i=0;i<nb_subvect;i++)
556    {
557       float s=1;
558       if (signs[i])
559          s=-1;
560       for (j=0;j<subvect_size;j++)
561          exc[subvect_size*i+j]+=s*shape_cb[ind[i]*subvect_size+j];
562    }
563    POP(stack);
564    POP(stack);
565 }
566
567 void noise_codebook_quant(
568 float target[],                 /* target vector */
569 float ak[],                     /* LPCs for this subframe */
570 float awk1[],                   /* Weighted LPCs for this subframe */
571 float awk2[],                   /* Weighted LPCs for this subframe */
572 void *par,                      /* Codebook/search parameters*/
573 int   p,                        /* number of LPC coeffs */
574 int   nsf,                      /* number of samples in subframe */
575 float *exc,
576 SpeexBits *bits,
577 float *stack,
578 int   complexity
579 )
580 {
581    int i;
582    float *tmp=PUSH(stack, nsf);
583    syn_filt_zero(target, awk1, tmp, nsf, p);
584    residue_zero(tmp, ak, tmp, nsf, p);
585    residue_zero(tmp, awk2, tmp, nsf, p);
586
587    for (i=0;i<nsf;i++)
588       exc[i]+=tmp[i];
589    for (i=0;i<nsf;i++)
590       target[i]=0;
591
592    POP(stack);
593 }
594
595
596 void noise_codebook_unquant(
597 float *exc,
598 void *par,                      /* non-overlapping codebook */
599 int   nsf,                      /* number of samples in subframe */
600 SpeexBits *bits,
601 float *stack
602 )
603 {
604    int i;
605
606    for (i=0;i<nsf;i++)
607       exc[i]+=3*((((float)rand())/RAND_MAX)-.5);
608 }