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