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