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