Cleaned up the pre-echo avoidance code so it works when compiled as fixed-point
[opus.git] / libcelt / bands.c
1 /* (C) 2007-2008 Jean-Marc Valin, CSIRO
2 */
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 <math.h>
37 #include "bands.h"
38 #include "modes.h"
39 #include "vq.h"
40 #include "cwrs.h"
41 #include "stack_alloc.h"
42 #include "os_support.h"
43 #include "mathops.h"
44
45 #if 0
46 void exp_rotation(celt_norm_t *X, int len, int dir, int stride, int iter)
47 {
48    int i, k;
49    celt_word16_t c, s;
50    celt_norm_t *Xptr;
51    /* Equivalent to cos(.3) and sin(.3) */
52    c = QCONST16(0.95534,15);
53    s = dir*QCONST16(0.29552,15);
54    for (k=0;k<iter;k++)
55    {
56       /* We could use MULT16_16_P15 instead of MULT16_16_Q15 for more accuracy, 
57          but at this point, I really don't think it's necessary */
58       Xptr = X;
59       for (i=0;i<len-stride;i++)
60       {
61          celt_norm_t x1, x2;
62          x1 = Xptr[0];
63          x2 = Xptr[stride];
64          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
65          *Xptr++      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
66       }
67       Xptr = &X[len-2*stride-1];
68       for (i=len-2*stride-1;i>=0;i--)
69       {
70          celt_norm_t x1, x2;
71          x1 = Xptr[0];
72          x2 = Xptr[stride];
73          Xptr[stride] = MULT16_16_Q15(c,x2) + MULT16_16_Q15(s,x1);
74          *Xptr--      = MULT16_16_Q15(c,x1) - MULT16_16_Q15(s,x2);
75       }
76    }
77 }
78 #endif
79
80
81 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
82
83 #ifdef FIXED_POINT
84 /* Compute the amplitude (sqrt energy) in each of the bands */
85 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
86 {
87    int i, c;
88    const celt_int16_t *eBands = m->eBands;
89    const int C = CHANNELS(m);
90    for (c=0;c<C;c++)
91    {
92       for (i=0;i<m->nbEBands;i++)
93       {
94          int j;
95          celt_word32_t maxval=0;
96          celt_word32_t sum = 0;
97          
98          j=eBands[i]; do {
99             maxval = MAX32(maxval, X[j*C+c]);
100             maxval = MAX32(maxval, -X[j*C+c]);
101          } while (++j<eBands[i+1]);
102          
103          if (maxval > 0)
104          {
105             int shift = celt_ilog2(maxval)-10;
106             j=eBands[i]; do {
107                sum += MULT16_16(EXTRACT16(VSHR32(X[j*C+c],shift)),
108                                 EXTRACT16(VSHR32(X[j*C+c],shift)));
109             } while (++j<eBands[i+1]);
110             /* We're adding one here to make damn sure we never end up with a pitch vector that's
111                larger than unity norm */
112             bank[i*C+c] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
113          } else {
114             bank[i*C+c] = EPSILON;
115          }
116          /*printf ("%f ", bank[i*C+c]);*/
117       }
118    }
119    /*printf ("\n");*/
120 }
121
122 /* Normalise each band such that the energy is one. */
123 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
124 {
125    int i, c;
126    const celt_int16_t *eBands = m->eBands;
127    const int C = CHANNELS(m);
128    for (c=0;c<C;c++)
129    {
130       i=0; do {
131          celt_word16_t g;
132          int j,shift;
133          celt_word16_t E;
134          shift = celt_zlog2(bank[i*C+c])-13;
135          E = VSHR32(bank[i*C+c], shift);
136          g = EXTRACT16(celt_rcp(SHR32(MULT16_16(E,sqrtC_1[C-1]),11)));
137          j=eBands[i]; do {
138             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j*C+c],shift-1),g);
139          } while (++j<eBands[i+1]);
140       } while (++i<m->nbEBands);
141    }
142 }
143
144 #ifndef DISABLE_STEREO
145 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
146 {
147    int i;
148    VARDECL(celt_ener_t, tmpE);
149    VARDECL(celt_sig_t, freq);
150    SAVE_STACK;
151    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
152    ALLOC(freq, m->nbChannels*m->eBands[m->nbEBands+1], celt_sig_t);
153    for (i=0;i<m->nbChannels*m->eBands[m->nbEBands+1];i++)
154       freq[i] = SHL32(EXTEND32(X[i]), 10);
155    compute_band_energies(m, freq, tmpE);
156    normalise_bands(m, freq, X, tmpE);
157    RESTORE_STACK;
158 }
159 #endif /* DISABLE_STEREO */
160 #else /* FIXED_POINT */
161 /* Compute the amplitude (sqrt energy) in each of the bands */
162 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
163 {
164    int i, c;
165    const celt_int16_t *eBands = m->eBands;
166    const int C = CHANNELS(m);
167    for (c=0;c<C;c++)
168    {
169       for (i=0;i<m->nbEBands;i++)
170       {
171          int j;
172          celt_word32_t sum = 1e-10;
173          for (j=eBands[i];j<eBands[i+1];j++)
174             sum += X[j*C+c]*X[j*C+c];
175          bank[i*C+c] = sqrt(sum);
176          /*printf ("%f ", bank[i*C+c]);*/
177       }
178    }
179    /*printf ("\n");*/
180 }
181
182 /* Normalise each band such that the energy is one. */
183 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
184 {
185    int i, c;
186    const celt_int16_t *eBands = m->eBands;
187    const int C = CHANNELS(m);
188    for (c=0;c<C;c++)
189    {
190       for (i=0;i<m->nbEBands;i++)
191       {
192          int j;
193          celt_word16_t g = 1.f/(1e-10+bank[i*C+c]*sqrt(C));
194          for (j=eBands[i];j<eBands[i+1];j++)
195             X[j*C+c] = freq[j*C+c]*g;
196       }
197    }
198 }
199
200 #ifndef DISABLE_STEREO
201 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
202 {
203    VARDECL(celt_ener_t, tmpE);
204    SAVE_STACK;
205    ALLOC(tmpE, m->nbEBands*m->nbChannels, celt_ener_t);
206    compute_band_energies(m, X, tmpE);
207    normalise_bands(m, X, X, tmpE);
208    RESTORE_STACK;
209 }
210 #endif /* DISABLE_STEREO */
211 #endif /* FIXED_POINT */
212
213 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
214 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
215 {
216    int i, c;
217    const celt_int16_t *eBands = m->eBands;
218    const int C = CHANNELS(m);
219    if (C>2)
220       celt_fatal("denormalise_bands() not implemented for >2 channels");
221    for (c=0;c<C;c++)
222    {
223       for (i=0;i<m->nbEBands;i++)
224       {
225          int j;
226          celt_word32_t g = MULT16_32_Q13(sqrtC_1[C-1],bank[i*C+c]);
227          j=eBands[i]; do {
228             freq[j*C+c] = MULT16_32_Q15(X[j*C+c], g);
229          } while (++j<eBands[i+1]);
230       }
231    }
232    for (i=C*eBands[m->nbEBands];i<C*eBands[m->nbEBands+1];i++)
233       freq[i] = 0;
234 }
235
236
237 /* Compute the best gain for each "pitch band" */
238 void compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
239 {
240    int i;
241    const celt_int16_t *pBands = m->pBands;
242    const int C = CHANNELS(m);
243
244    for (i=0;i<m->nbPBands;i++)
245    {
246       celt_word32_t Sxy=0, Sxx=0;
247       int j;
248       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
249       for (j=C*pBands[i];j<C*pBands[i+1];j++)
250       {
251          Sxy = MAC16_16(Sxy, X[j], P[j]);
252          Sxx = MAC16_16(Sxx, X[j], X[j]);
253       }
254       /* No negative gain allowed */
255       if (Sxy < 0)
256          Sxy = 0;
257       /* Not sure how that would happen, just making sure */
258       if (Sxy > Sxx)
259          Sxy = Sxx;
260       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
261          residual doesn't quantise well */
262       Sxy = MULT16_32_Q15(QCONST16(.9f, 15), Sxy);
263       /* gain = Sxy/Sxx */
264       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
265       /*printf ("%f ", 1-sqrt(1-gain*gain));*/
266    }
267    /*if(rand()%10==0)
268    {
269       for (i=0;i<m->nbPBands;i++)
270          printf ("%f ", 1-sqrt(1-gains[i]*gains[i]));
271       printf ("\n");
272    }*/
273 }
274
275 /* Apply the (quantised) gain to each "pitch band" */
276 void pitch_quant_bands(const CELTMode *m, celt_norm_t * restrict P, const celt_pgain_t * restrict gains)
277 {
278    int i;
279    const celt_int16_t *pBands = m->pBands;
280    const int C = CHANNELS(m);
281    for (i=0;i<m->nbPBands;i++)
282    {
283       int j;
284       for (j=C*pBands[i];j<C*pBands[i+1];j++)
285          P[j] = MULT16_16_Q15(gains[i], P[j]);
286       /*printf ("%f ", gain);*/
287    }
288    for (i=C*pBands[m->nbPBands];i<C*pBands[m->nbPBands+1];i++)
289       P[i] = 0;
290 }
291
292 static void intensity_band(celt_norm_t * restrict X, int len)
293 {
294    int j;
295    celt_word32_t E = 1e-15;
296    celt_word32_t E2 = 1e-15;
297    for (j=0;j<len;j++)
298    {
299       X[j] = X[2*j];
300       E += MULT16_16(X[j],X[j]);
301       E2 += MULT16_16(X[2*j+1],X[2*j+1]);
302    }
303 #ifndef FIXED_POINT
304    E  = celt_sqrt(E+E2)/celt_sqrt(E);
305    for (j=0;j<len;j++)
306       X[j] *= E;
307 #endif
308    for (j=0;j<len;j++)
309       X[len+j] = 0;
310
311 }
312
313 static void dup_band(celt_norm_t * restrict X, int len)
314 {
315    int j;
316    for (j=len-1;j>=0;j--)
317    {
318       X[2*j] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
319       X[2*j+1] = MULT16_16_Q15(QCONST16(.70711f,15),X[j]);
320    }
321 }
322
323 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, const int *stereo_mode, int bandID, int dir)
324 {
325    int i = bandID;
326    const celt_int16_t *eBands = m->eBands;
327    const int C = CHANNELS(m);
328    {
329       int j;
330       if (stereo_mode[i] && dir <0)
331       {
332          dup_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
333       } else {
334          celt_word16_t a1, a2;
335          if (stereo_mode[i]==0)
336          {
337             /* Do mid-side when not doing intensity stereo */
338             a1 = QCONST16(.70711f,14);
339             a2 = dir*QCONST16(.70711f,14);
340          } else {
341             celt_word16_t left, right;
342             celt_word16_t norm;
343 #ifdef FIXED_POINT
344             int shift = celt_zlog2(MAX32(bank[i*C], bank[i*C+1]))-13;
345 #endif
346             left = VSHR32(bank[i*C],shift);
347             right = VSHR32(bank[i*C+1],shift);
348             norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
349             a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
350             a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
351          }
352          for (j=eBands[i];j<eBands[i+1];j++)
353          {
354             celt_norm_t r, l;
355             l = X[j*C];
356             r = X[j*C+1];
357             X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
358             X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
359          }
360       }
361       if (stereo_mode[i] && dir>0)
362       {
363          intensity_band(X+C*eBands[i], eBands[i+1]-eBands[i]);
364       }
365    }
366 }
367
368 void stereo_decision(const CELTMode *m, celt_norm_t * restrict X, int *stereo_mode, int len)
369 {
370    int i;
371    for (i=0;i<len-5;i++)
372       stereo_mode[i] = 0;
373    for (;i<len;i++)
374       stereo_mode[i] = 0;
375 }
376
377
378
379 /* Quantisation of the residual */
380 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, const celt_ener_t *bandE, const int *stereo_mode, int total_bits, int time_domain, ec_enc *enc)
381 {
382    int i, j, bits;
383    const celt_int16_t * restrict eBands = m->eBands;
384    celt_norm_t * restrict norm;
385    VARDECL(celt_norm_t, _norm);
386    VARDECL(int, pulses);
387    VARDECL(int, offsets);
388    const int C = CHANNELS(m);
389    SAVE_STACK;
390
391    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
392    ALLOC(pulses, m->nbEBands, int);
393    ALLOC(offsets, m->nbEBands, int);
394    norm = _norm;
395
396    for (i=0;i<m->nbEBands;i++)
397       offsets[i] = 0;
398    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
399    bits = total_bits - ec_enc_tell(enc, 0) - 1;
400    compute_allocation(m, offsets, stereo_mode, bits, pulses);
401    
402    /*printf("bits left: %d\n", bits);
403    for (i=0;i<m->nbEBands;i++)
404       printf ("%d ", pulses[i]);
405    printf ("\n");*/
406    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
407    for (i=0;i<m->nbEBands;i++)
408    {
409       int q;
410       celt_word16_t n;
411       q = pulses[i];
412       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
413
414       /* If pitch isn't available, use intra-frame prediction */
415       if (eBands[i] >= m->pitchEnd || q<=0)
416       {
417          q -= 1;
418          if (!time_domain)
419          {
420             if (q<0)
421                intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
422             else
423                intra_prediction(m, X+C*eBands[i], W+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1], enc);
424          }
425       }
426       
427       if (q > 0)
428       {
429          int ch=C;
430          if (C==2 && stereo_mode[i]==1)
431             ch = 1;
432          if (C==2)
433          {
434             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
435             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
436          }
437          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
438          if (C==2)
439             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
440       } else {
441          for (j=C*eBands[i];j<C*eBands[i+1];j++)
442             X[j] = P[j];
443       }
444       for (j=C*eBands[i];j<C*eBands[i+1];j++)
445          norm[j] = MULT16_16_Q15(n,X[j]);
446    }
447    RESTORE_STACK;
448 }
449
450 /* Decoding of the residual */
451 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, const int *stereo_mode, int total_bits, int time_domain, ec_dec *dec)
452 {
453    int i, j, bits;
454    const celt_int16_t * restrict eBands = m->eBands;
455    celt_norm_t * restrict norm;
456    VARDECL(celt_norm_t, _norm);
457    VARDECL(int, pulses);
458    VARDECL(int, offsets);
459    const int C = CHANNELS(m);
460    SAVE_STACK;
461
462    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
463    ALLOC(pulses, m->nbEBands, int);
464    ALLOC(offsets, m->nbEBands, int);
465    norm = _norm;
466
467    for (i=0;i<m->nbEBands;i++)
468       offsets[i] = 0;
469    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
470    bits = total_bits - ec_dec_tell(dec, 0) - 1;
471    compute_allocation(m, offsets, stereo_mode, bits, pulses);
472
473    for (i=0;i<m->nbEBands;i++)
474    {
475       int q;
476       celt_word16_t n;
477       q = pulses[i];
478       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
479
480       /* If pitch isn't available, use intra-frame prediction */
481       if (eBands[i] >= m->pitchEnd || q<=0)
482       {
483          q -= 1;
484          if (!time_domain)
485          {
486             if (q<0)
487                intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1]);
488             else
489                intra_unquant(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], eBands[m->nbEBands+1], dec);
490          }
491       }
492       
493       if (q > 0)
494       {
495          int ch=C;
496          if (C==2 && stereo_mode[i]==1)
497             ch = 1;
498          if (C==2)
499             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
500          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
501          if (C==2)
502             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
503       } else {
504          for (j=C*eBands[i];j<C*eBands[i+1];j++)
505             X[j] = P[j];
506       }
507       for (j=C*eBands[i];j<C*eBands[i+1];j++)
508          norm[j] = MULT16_16_Q15(n,X[j]);
509    }
510    RESTORE_STACK;
511 }
512