Re-enabling folding/intra for transients
[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 shortBlocks, 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    int B;
390    SAVE_STACK;
391
392    B = shortBlocks ? m->nbShortMdcts : 1;
393    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
394    ALLOC(pulses, m->nbEBands, int);
395    ALLOC(offsets, m->nbEBands, int);
396    norm = _norm;
397
398    for (i=0;i<m->nbEBands;i++)
399       offsets[i] = 0;
400    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
401    bits = total_bits - ec_enc_tell(enc, 0) - 1;
402    compute_allocation(m, offsets, stereo_mode, bits, pulses);
403    
404    /*printf("bits left: %d\n", bits);
405    for (i=0;i<m->nbEBands;i++)
406       printf ("%d ", pulses[i]);
407    printf ("\n");*/
408    /*printf ("%d %d\n", ec_enc_tell(enc, 0), compute_allocation(m, m->nbPulses));*/
409    for (i=0;i<m->nbEBands;i++)
410    {
411       int q;
412       celt_word16_t n;
413       q = pulses[i];
414       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
415
416       /* If pitch isn't available, use intra-frame prediction */
417       if (eBands[i] >= m->pitchEnd || q<=0)
418       {
419          q -= 1;
420          if (q<0)
421             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
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], B, enc);
424       }
425       
426       if (q > 0)
427       {
428          int ch=C;
429          if (C==2 && stereo_mode[i]==1)
430             ch = 1;
431          if (C==2)
432          {
433             stereo_band_mix(m, X, bandE, stereo_mode, i, 1);
434             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
435          }
436          alg_quant(X+C*eBands[i], W+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], enc);
437          if (C==2)
438             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
439       } else {
440          for (j=C*eBands[i];j<C*eBands[i+1];j++)
441             X[j] = P[j];
442       }
443       for (j=C*eBands[i];j<C*eBands[i+1];j++)
444          norm[j] = MULT16_16_Q15(n,X[j]);
445    }
446    RESTORE_STACK;
447 }
448
449 /* Decoding of the residual */
450 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 shortBlocks, ec_dec *dec)
451 {
452    int i, j, bits;
453    const celt_int16_t * restrict eBands = m->eBands;
454    celt_norm_t * restrict norm;
455    VARDECL(celt_norm_t, _norm);
456    VARDECL(int, pulses);
457    VARDECL(int, offsets);
458    const int C = CHANNELS(m);
459    int B;
460    SAVE_STACK;
461
462    B = shortBlocks ? m->nbShortMdcts : 1;
463    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
464    ALLOC(pulses, m->nbEBands, int);
465    ALLOC(offsets, m->nbEBands, int);
466    norm = _norm;
467
468    for (i=0;i<m->nbEBands;i++)
469       offsets[i] = 0;
470    /* Use a single-bit margin to guard against overrunning (make sure it's enough) */
471    bits = total_bits - ec_dec_tell(dec, 0) - 1;
472    compute_allocation(m, offsets, stereo_mode, bits, pulses);
473
474    for (i=0;i<m->nbEBands;i++)
475    {
476       int q;
477       celt_word16_t n;
478       q = pulses[i];
479       n = SHL16(celt_sqrt(C*(eBands[i+1]-eBands[i])),11);
480
481       /* If pitch isn't available, use intra-frame prediction */
482       if (eBands[i] >= m->pitchEnd || q<=0)
483       {
484          q -= 1;
485          if (q<0)
486             intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
487          else
488             intra_unquant(m, X+C*eBands[i], eBands[i+1]-eBands[i], q, norm, P+C*eBands[i], eBands[i], B, dec);
489       }
490       
491       if (q > 0)
492       {
493          int ch=C;
494          if (C==2 && stereo_mode[i]==1)
495             ch = 1;
496          if (C==2)
497             stereo_band_mix(m, P, bandE, stereo_mode, i, 1);
498          alg_unquant(X+C*eBands[i], ch*(eBands[i+1]-eBands[i]), q, P+C*eBands[i], dec);
499          if (C==2)
500             stereo_band_mix(m, X, bandE, stereo_mode, i, -1);
501       } else {
502          for (j=C*eBands[i];j<C*eBands[i+1];j++)
503             X[j] = P[j];
504       }
505       for (j=C*eBands[i];j<C*eBands[i+1];j++)
506          norm[j] = MULT16_16_Q15(n,X[j]);
507    }
508    RESTORE_STACK;
509 }
510