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