prefilter/postfilter now forced off in Opus hybrid mode
[opus.git] / libcelt / bands.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2009 Xiph.Org Foundation
3    Copyright (c) 2008-2009 Gregory Maxwell 
4    Written by Jean-Marc Valin and Gregory Maxwell */
5 /*
6    Redistribution and use in source and binary forms, with or without
7    modification, are permitted provided that the following conditions
8    are met:
9    
10    - Redistributions of source code must retain the above copyright
11    notice, this list of conditions and the following disclaimer.
12    
13    - Redistributions in binary form must reproduce the above copyright
14    notice, this list of conditions and the following disclaimer in the
15    documentation and/or other materials provided with the distribution.
16    
17    - Neither the name of the Xiph.org Foundation nor the names of its
18    contributors may be used to endorse or promote products derived from
19    this software without specific prior written permission.
20    
21    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
25    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
28    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
29    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
30    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32 */
33
34 #ifdef HAVE_CONFIG_H
35 #include "config.h"
36 #endif
37
38 #include <math.h>
39 #include "bands.h"
40 #include "modes.h"
41 #include "vq.h"
42 #include "cwrs.h"
43 #include "stack_alloc.h"
44 #include "os_support.h"
45 #include "mathops.h"
46 #include "rate.h"
47
48 celt_uint32 lcg_rand(celt_uint32 seed)
49 {
50    return 1664525 * seed + 1013904223;
51 }
52
53 /* This is a cos() approximation designed to be bit-exact on any platform. Bit exactness
54    with this approximation is important because it has an impact on the bit allocation */
55 static celt_int16 bitexact_cos(celt_int16 x)
56 {
57    celt_int32 tmp;
58    celt_int16 x2;
59    tmp = (4096+((celt_int32)(x)*(x)))>>13;
60    if (tmp > 32767)
61       tmp = 32767;
62    x2 = tmp;
63    x2 = (32767-x2) + FRAC_MUL16(x2, (-7651 + FRAC_MUL16(x2, (8277 + FRAC_MUL16(-626, x2)))));
64    if (x2 > 32766)
65       x2 = 32766;
66    return 1+x2;
67 }
68
69 static int bitexact_log2tan(int isin,int icos)
70 {
71    int lc;
72    int ls;
73    lc=EC_ILOG(icos);
74    ls=EC_ILOG(isin);
75    icos<<=15-lc;
76    isin<<=15-ls;
77    return (ls-lc<<11)
78          +FRAC_MUL16(isin, FRAC_MUL16(isin, -2597) + 7932)
79          -FRAC_MUL16(icos, FRAC_MUL16(icos, -2597) + 7932);
80 }
81
82 #ifdef FIXED_POINT
83 /* Compute the amplitude (sqrt energy) in each of the bands */
84 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int end, int _C, int M)
85 {
86    int i, c, N;
87    const celt_int16 *eBands = m->eBands;
88    const int C = CHANNELS(_C);
89    N = M*m->shortMdctSize;
90    c=0; do {
91       for (i=0;i<end;i++)
92       {
93          int j;
94          celt_word32 maxval=0;
95          celt_word32 sum = 0;
96          
97          j=M*eBands[i]; do {
98             maxval = MAX32(maxval, X[j+c*N]);
99             maxval = MAX32(maxval, -X[j+c*N]);
100          } while (++j<M*eBands[i+1]);
101          
102          if (maxval > 0)
103          {
104             int shift = celt_ilog2(maxval)-10;
105             j=M*eBands[i]; do {
106                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
107                                    EXTRACT16(VSHR32(X[j+c*N],shift)));
108             } while (++j<M*eBands[i+1]);
109             /* We're adding one here to make damn sure we never end up with a pitch vector that's
110                larger than unity norm */
111             bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
112          } else {
113             bank[i+c*m->nbEBands] = EPSILON;
114          }
115          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
116       }
117    } while (++c<C);
118    /*printf ("\n");*/
119 }
120
121 /* Normalise each band such that the energy is one. */
122 void normalise_bands(const CELTMode *m, const celt_sig * restrict freq, celt_norm * restrict X, const celt_ener *bank, int end, int _C, int M)
123 {
124    int i, c, N;
125    const celt_int16 *eBands = m->eBands;
126    const int C = CHANNELS(_C);
127    N = M*m->shortMdctSize;
128    c=0; do {
129       i=0; do {
130          celt_word16 g;
131          int j,shift;
132          celt_word16 E;
133          shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
134          E = VSHR32(bank[i+c*m->nbEBands], shift);
135          g = EXTRACT16(celt_rcp(SHL32(E,3)));
136          j=M*eBands[i]; do {
137             X[j+c*N] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
138          } while (++j<M*eBands[i+1]);
139       } while (++i<end);
140    } while (++c<C);
141 }
142
143 #else /* FIXED_POINT */
144 /* Compute the amplitude (sqrt energy) in each of the bands */
145 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int end, int _C, int M)
146 {
147    int i, c, N;
148    const celt_int16 *eBands = m->eBands;
149    const int C = CHANNELS(_C);
150    N = M*m->shortMdctSize;
151    c=0; do {
152       for (i=0;i<end;i++)
153       {
154          int j;
155          celt_word32 sum = 1e-27f;
156          for (j=M*eBands[i];j<M*eBands[i+1];j++)
157             sum += X[j+c*N]*X[j+c*N];
158          bank[i+c*m->nbEBands] = celt_sqrt(sum);
159          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
160       }
161    } while (++c<C);
162    /*printf ("\n");*/
163 }
164
165 /* Normalise each band such that the energy is one. */
166 void normalise_bands(const CELTMode *m, const celt_sig * restrict freq, celt_norm * restrict X, const celt_ener *bank, int end, int _C, int M)
167 {
168    int i, c, N;
169    const celt_int16 *eBands = m->eBands;
170    const int C = CHANNELS(_C);
171    N = M*m->shortMdctSize;
172    c=0; do {
173       for (i=0;i<end;i++)
174       {
175          int j;
176          celt_word16 g = 1.f/(1e-27f+bank[i+c*m->nbEBands]);
177          for (j=M*eBands[i];j<M*eBands[i+1];j++)
178             X[j+c*N] = freq[j+c*N]*g;
179       }
180    } while (++c<C);
181 }
182
183 #endif /* FIXED_POINT */
184
185 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
186 void denormalise_bands(const CELTMode *m, const celt_norm * restrict X, celt_sig * restrict freq, const celt_ener *bank, int end, int _C, int M)
187 {
188    int i, c, N;
189    const celt_int16 *eBands = m->eBands;
190    const int C = CHANNELS(_C);
191    N = M*m->shortMdctSize;
192    celt_assert2(C<=2, "denormalise_bands() not implemented for >2 channels");
193    c=0; do {
194       celt_sig * restrict f;
195       const celt_norm * restrict x;
196       f = freq+c*N;
197       x = X+c*N;
198       for (i=0;i<end;i++)
199       {
200          int j, band_end;
201          celt_word32 g = SHR32(bank[i+c*m->nbEBands],1);
202          j=M*eBands[i];
203          band_end = M*eBands[i+1];
204          do {
205             *f++ = SHL32(MULT16_32_Q15(*x, g),2);
206             x++;
207          } while (++j<band_end);
208       }
209       for (i=M*eBands[m->nbEBands];i<N;i++)
210          *f++ = 0;
211    } while (++c<C);
212 }
213
214 /* This prevents energy collapse for transients with multiple short MDCTs */
215 void anti_collapse(const CELTMode *m, celt_norm *_X, unsigned char *collapse_masks, int LM, int C, int size,
216       int start, int end, celt_word16 *logE, celt_word16 *prev1logE,
217       celt_word16 *prev2logE, int *pulses, celt_uint32 seed)
218 {
219    int c, i, j, k;
220    for (i=start;i<end;i++)
221    {
222       int N0;
223       celt_word16 thresh, sqrt_1;
224       int depth;
225 #ifdef FIXED_POINT
226       int shift;
227 #endif
228
229       N0 = m->eBands[i+1]-m->eBands[i];
230       depth = (1+(pulses[i]>>BITRES))/(m->eBands[i+1]-m->eBands[i]<<LM);
231
232 #ifdef FIXED_POINT
233       thresh = MULT16_32_Q15(QCONST16(0.3f, 15), MIN32(32767,SHR32(celt_exp2(-SHL16(depth, 11)),1) ));
234       {
235          celt_word32 t;
236          t = N0<<LM;
237          shift = celt_ilog2(t)>>1;
238          t = SHL32(t, (7-shift)<<1);
239          sqrt_1 = celt_rsqrt_norm(t);
240       }
241 #else
242       thresh = .3f*celt_exp2(-depth);
243       sqrt_1 = celt_rsqrt(N0<<LM);
244 #endif
245
246       c=0; do
247       {
248          celt_norm *X;
249          celt_word16 Ediff;
250          celt_word16 r;
251          Ediff = logE[c*m->nbEBands+i]-MIN16(prev1logE[c*m->nbEBands+i],prev2logE[c*m->nbEBands+i]);
252          Ediff = MAX16(0, Ediff);
253
254 #ifdef FIXED_POINT
255          if (Ediff < 16384)
256             r = 2*MIN16(16383,SHR32(celt_exp2(-SHL16(Ediff, 11-DB_SHIFT)),1));
257          else
258             r = 0;
259          r = SHR16(MIN16(thresh, r),1);
260          r = SHR32(MULT16_16_Q15(sqrt_1, r),shift);
261 #else
262          r = 2.f*celt_exp2(-Ediff);
263          r = MIN16(thresh, r);
264          r = r*sqrt_1;
265 #endif
266          X = _X+c*size+(m->eBands[i]<<LM);
267          for (k=0;k<1<<LM;k++)
268          {
269             /* Detect collapse */
270             if (!(collapse_masks[i*C+c]&1<<k))
271             {
272                /* Fill with noise */
273                for (j=0;j<N0;j++)
274                {
275                   seed = lcg_rand(seed);
276                   X[(j<<LM)+k] = (seed&0x8000 ? r : -r);
277                }
278             }
279          }
280          /* We just added some energy, so we need to renormalise */
281          renormalise_vector(X, N0<<LM, Q15ONE);
282       } while (++c<C);
283    }
284
285 }
286
287
288 static void intensity_stereo(const CELTMode *m, celt_norm *X, celt_norm *Y, const celt_ener *bank, int bandID, int N)
289 {
290    int i = bandID;
291    int j;
292    celt_word16 a1, a2;
293    celt_word16 left, right;
294    celt_word16 norm;
295 #ifdef FIXED_POINT
296    int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
297 #endif
298    left = VSHR32(bank[i],shift);
299    right = VSHR32(bank[i+m->nbEBands],shift);
300    norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
301    a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
302    a2 = DIV32_16(SHL32(EXTEND32(right),14),norm);
303    for (j=0;j<N;j++)
304    {
305       celt_norm r, l;
306       l = X[j];
307       r = Y[j];
308       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
309       /* Side is not encoded, no need to calculate */
310    }
311 }
312
313 static void stereo_split(celt_norm *X, celt_norm *Y, int N)
314 {
315    int j;
316    for (j=0;j<N;j++)
317    {
318       celt_norm r, l;
319       l = MULT16_16_Q15(QCONST16(.70710678f,15), X[j]);
320       r = MULT16_16_Q15(QCONST16(.70710678f,15), Y[j]);
321       X[j] = l+r;
322       Y[j] = r-l;
323    }
324 }
325
326 static void stereo_merge(celt_norm *X, celt_norm *Y, celt_word16 mid, int N)
327 {
328    int j;
329    celt_word32 xp=0, side=0;
330    celt_word32 El, Er;
331    celt_word16 mid2;
332 #ifdef FIXED_POINT
333    int kl, kr;
334 #endif
335    celt_word32 t, lgain, rgain;
336
337    /* Compute the norm of X+Y and X-Y as |X|^2 + |Y|^2 +/- sum(xy) */
338    for (j=0;j<N;j++)
339    {
340       xp = MAC16_16(xp, X[j], Y[j]);
341       side = MAC16_16(side, Y[j], Y[j]);
342    }
343    /* Compensating for the mid normalization */
344    xp = MULT16_32_Q15(mid, xp);
345    /* mid and side are in Q15, not Q14 like X and Y */
346    mid2 = SHR32(mid, 1);
347    El = MULT16_16(mid2, mid2) + side - 2*xp;
348    Er = MULT16_16(mid2, mid2) + side + 2*xp;
349    if (Er < QCONST32(6e-4f, 28) || El < QCONST32(6e-4f, 28))
350    {
351       for (j=0;j<N;j++)
352          Y[j] = X[j];
353       return;
354    }
355
356 #ifdef FIXED_POINT
357    kl = celt_ilog2(El)>>1;
358    kr = celt_ilog2(Er)>>1;
359 #endif
360    t = VSHR32(El, (kl-7)<<1);
361    lgain = celt_rsqrt_norm(t);
362    t = VSHR32(Er, (kr-7)<<1);
363    rgain = celt_rsqrt_norm(t);
364
365 #ifdef FIXED_POINT
366    if (kl < 7)
367       kl = 7;
368    if (kr < 7)
369       kr = 7;
370 #endif
371
372    for (j=0;j<N;j++)
373    {
374       celt_norm r, l;
375       /* Apply mid scaling (side is already scaled) */
376       l = MULT16_16_Q15(mid, X[j]);
377       r = Y[j];
378       X[j] = EXTRACT16(PSHR32(MULT16_16(lgain, SUB16(l,r)), kl+1));
379       Y[j] = EXTRACT16(PSHR32(MULT16_16(rgain, ADD16(l,r)), kr+1));
380    }
381 }
382
383 /* Decide whether we should spread the pulses in the current frame */
384 int spreading_decision(const CELTMode *m, celt_norm *X, int *average,
385       int last_decision, int *hf_average, int *tapset_decision, int update_hf,
386       int end, int _C, int M)
387 {
388    int i, c, N0;
389    int sum = 0, nbBands=0;
390    const int C = CHANNELS(_C);
391    const celt_int16 * restrict eBands = m->eBands;
392    int decision;
393    int hf_sum=0;
394    
395    N0 = M*m->shortMdctSize;
396
397    if (M*(eBands[end]-eBands[end-1]) <= 8)
398       return SPREAD_NONE;
399    c=0; do {
400       for (i=0;i<end;i++)
401       {
402          int j, N, tmp=0;
403          int tcount[3] = {0};
404          celt_norm * restrict x = X+M*eBands[i]+c*N0;
405          N = M*(eBands[i+1]-eBands[i]);
406          if (N<=8)
407             continue;
408          /* Compute rough CDF of |x[j]| */
409          for (j=0;j<N;j++)
410          {
411             celt_word32 x2N; /* Q13 */
412
413             x2N = MULT16_16(MULT16_16_Q15(x[j], x[j]), N);
414             if (x2N < QCONST16(0.25f,13))
415                tcount[0]++;
416             if (x2N < QCONST16(0.0625f,13))
417                tcount[1]++;
418             if (x2N < QCONST16(0.015625f,13))
419                tcount[2]++;
420          }
421
422          /* Only include four last bands (8 kHz and up) */
423          if (i>m->nbEBands-4)
424             hf_sum += 32*(tcount[1]+tcount[0])/N;
425          tmp = (2*tcount[2] >= N) + (2*tcount[1] >= N) + (2*tcount[0] >= N);
426          sum += tmp*256;
427          nbBands++;
428       }
429    } while (++c<C);
430
431    if (update_hf)
432    {
433       if (hf_sum)
434          hf_sum /= C*(4-m->nbEBands+end);
435       *hf_average = (*hf_average+hf_sum)>>1;
436       hf_sum = *hf_average;
437       if (*tapset_decision==2)
438          hf_sum += 4;
439       else if (*tapset_decision==0)
440          hf_sum -= 4;
441       if (hf_sum > 22)
442          *tapset_decision=2;
443       else if (hf_sum > 18)
444          *tapset_decision=1;
445       else
446          *tapset_decision=0;
447    }
448    /*printf("%d %d %d\n", hf_sum, *hf_average, *tapset_decision);*/
449    sum /= nbBands;
450    /* Recursive averaging */
451    sum = (sum+*average)>>1;
452    *average = sum;
453    /* Hysteresis */
454    sum = (3*sum + (((3-last_decision)<<7) + 64) + 2)>>2;
455    if (sum < 80)
456    {
457       decision = SPREAD_AGGRESSIVE;
458    } else if (sum < 256)
459    {
460       decision = SPREAD_NORMAL;
461    } else if (sum < 384)
462    {
463       decision = SPREAD_LIGHT;
464    } else {
465       decision = SPREAD_NONE;
466    }
467    return decision;
468 }
469
470 #ifdef MEASURE_NORM_MSE
471
472 float MSE[30] = {0};
473 int nbMSEBands = 0;
474 int MSECount[30] = {0};
475
476 void dump_norm_mse(void)
477 {
478    int i;
479    for (i=0;i<nbMSEBands;i++)
480    {
481       printf ("%g ", MSE[i]/MSECount[i]);
482    }
483    printf ("\n");
484 }
485
486 void measure_norm_mse(const CELTMode *m, float *X, float *X0, float *bandE, float *bandE0, int M, int N, int C)
487 {
488    static int init = 0;
489    int i;
490    if (!init)
491    {
492       atexit(dump_norm_mse);
493       init = 1;
494    }
495    for (i=0;i<m->nbEBands;i++)
496    {
497       int j;
498       int c;
499       float g;
500       if (bandE0[i]<10 || (C==2 && bandE0[i+m->nbEBands]<1))
501          continue;
502       c=0; do {
503          g = bandE[i+c*m->nbEBands]/(1e-15+bandE0[i+c*m->nbEBands]);
504          for (j=M*m->eBands[i];j<M*m->eBands[i+1];j++)
505             MSE[i] += (g*X[j+c*N]-X0[j+c*N])*(g*X[j+c*N]-X0[j+c*N]);
506       } while (++c<C);
507       MSECount[i]+=C;
508    }
509    nbMSEBands = m->nbEBands;
510 }
511
512 #endif
513
514 /* Indexing table for converting from natural Hadamard to ordery Hadamard
515    This is essentially a bit-reversed Gray, on top of which we've added
516    an inversion of the order because we want the DC at the end rather than
517    the beginning. The lines are for N=2, 4, 8, 16 */
518 static const int ordery_table[] = {
519        1,  0,
520        3,  0,  2,  1,
521        7,  0,  4,  3,  6,  1,  5,  2,
522       15,  0,  8,  7, 12,  3, 11,  4, 14,  1,  9,  6, 13,  2, 10,  5,
523 };
524
525 static void deinterleave_hadamard(celt_norm *X, int N0, int stride, int hadamard)
526 {
527    int i,j;
528    VARDECL(celt_norm, tmp);
529    int N;
530    SAVE_STACK;
531    N = N0*stride;
532    ALLOC(tmp, N, celt_norm);
533    if (hadamard)
534    {
535       const int *ordery = ordery_table+stride-2;
536       for (i=0;i<stride;i++)
537       {
538          for (j=0;j<N0;j++)
539             tmp[ordery[i]*N0+j] = X[j*stride+i];
540       }
541    } else {
542       for (i=0;i<stride;i++)
543          for (j=0;j<N0;j++)
544             tmp[i*N0+j] = X[j*stride+i];
545    }
546    for (j=0;j<N;j++)
547       X[j] = tmp[j];
548    RESTORE_STACK;
549 }
550
551 static void interleave_hadamard(celt_norm *X, int N0, int stride, int hadamard)
552 {
553    int i,j;
554    VARDECL(celt_norm, tmp);
555    int N;
556    SAVE_STACK;
557    N = N0*stride;
558    ALLOC(tmp, N, celt_norm);
559    if (hadamard)
560    {
561       const int *ordery = ordery_table+stride-2;
562       for (i=0;i<stride;i++)
563          for (j=0;j<N0;j++)
564             tmp[j*stride+i] = X[ordery[i]*N0+j];
565    } else {
566       for (i=0;i<stride;i++)
567          for (j=0;j<N0;j++)
568             tmp[j*stride+i] = X[i*N0+j];
569    }
570    for (j=0;j<N;j++)
571       X[j] = tmp[j];
572    RESTORE_STACK;
573 }
574
575 void haar1(celt_norm *X, int N0, int stride)
576 {
577    int i, j;
578    N0 >>= 1;
579    for (i=0;i<stride;i++)
580       for (j=0;j<N0;j++)
581       {
582          celt_norm tmp1, tmp2;
583          tmp1 = MULT16_16_Q15(QCONST16(.70710678f,15), X[stride*2*j+i]);
584          tmp2 = MULT16_16_Q15(QCONST16(.70710678f,15), X[stride*(2*j+1)+i]);
585          X[stride*2*j+i] = tmp1 + tmp2;
586          X[stride*(2*j+1)+i] = tmp1 - tmp2;
587       }
588 }
589
590 static int compute_qn(int N, int b, int offset, int stereo)
591 {
592    static const celt_int16 exp2_table8[8] =
593       {16384, 17866, 19483, 21247, 23170, 25267, 27554, 30048};
594    int qn, qb;
595    int N2 = 2*N-1;
596    if (stereo && N==2)
597       N2--;
598    qb = IMIN((b>>1)-(1<<BITRES), (b+N2*offset)/N2);
599
600    qb = IMAX(0, IMIN(8<<BITRES, qb));
601
602    if (qb<(1<<BITRES>>1)) {
603       qn = 1;
604    } else {
605       qn = exp2_table8[qb&0x7]>>(14-(qb>>BITRES));
606       qn = (qn+1)>>1<<1;
607    }
608    celt_assert(qn <= 256);
609    return qn;
610 }
611
612 /* This function is responsible for encoding and decoding a band for both
613    the mono and stereo case. Even in the mono case, it can split the band
614    in two and transmit the energy difference with the two half-bands. It
615    can be called recursively so bands can end up being split in 8 parts. */
616 static unsigned quant_band(int encode, const CELTMode *m, int i, celt_norm *X, celt_norm *Y,
617       int N, int b, int spread, int B, int intensity, int tf_change, celt_norm *lowband, int resynth, void *ec,
618       celt_int32 *remaining_bits, int LM, celt_norm *lowband_out, const celt_ener *bandE, int level,
619       celt_uint32 *seed, celt_word16 gain, celt_norm *lowband_scratch, int fill)
620 {
621    int q;
622    int curr_bits;
623    int stereo, split;
624    int imid=0, iside=0;
625    int N0=N;
626    int N_B=N;
627    int N_B0;
628    int B0=B;
629    int time_divide=0;
630    int recombine=0;
631    int inv = 0;
632    celt_word16 mid=0, side=0;
633    int longBlocks;
634    unsigned cm=0;
635
636    longBlocks = B0==1;
637
638    N_B /= B;
639    N_B0 = N_B;
640
641    split = stereo = Y != NULL;
642
643    /* Special case for one sample */
644    if (N==1)
645    {
646       int c;
647       celt_norm *x = X;
648       c=0; do {
649          int sign=0;
650          if (*remaining_bits>=1<<BITRES)
651          {
652             if (encode)
653             {
654                sign = x[0]<0;
655                ec_enc_bits((ec_enc*)ec, sign, 1);
656             } else {
657                sign = ec_dec_bits((ec_dec*)ec, 1);
658             }
659             *remaining_bits -= 1<<BITRES;
660             b-=1<<BITRES;
661          }
662          if (resynth)
663             x[0] = sign ? -NORM_SCALING : NORM_SCALING;
664          x = Y;
665       } while (++c<1+stereo);
666       if (lowband_out)
667          lowband_out[0] = SHR16(X[0],4);
668       return 1;
669    }
670
671    if (!stereo && level == 0)
672    {
673       int k;
674       if (tf_change>0)
675          recombine = tf_change;
676       /* Band recombining to increase frequency resolution */
677
678       if (lowband && (recombine || ((N_B&1) == 0 && tf_change<0) || B0>1))
679       {
680          int j;
681          for (j=0;j<N;j++)
682             lowband_scratch[j] = lowband[j];
683          lowband = lowband_scratch;
684       }
685
686       for (k=0;k<recombine;k++)
687       {
688          if (encode)
689             haar1(X, N>>k, 1<<k);
690          if (lowband)
691             haar1(lowband, N>>k, 1<<k);
692          fill |= fill<<(1<<k);
693       }
694       B>>=recombine;
695       N_B<<=recombine;
696
697       /* Increasing the time resolution */
698       while ((N_B&1) == 0 && tf_change<0)
699       {
700          if (encode)
701             haar1(X, N_B, B);
702          if (lowband)
703             haar1(lowband, N_B, B);
704          fill |= fill<<B;
705          B <<= 1;
706          N_B >>= 1;
707          time_divide++;
708          tf_change++;
709       }
710       B0=B;
711       N_B0 = N_B;
712
713       /* Reorganize the samples in time order instead of frequency order */
714       if (B0>1)
715       {
716          if (encode)
717             deinterleave_hadamard(X, N_B>>recombine, B0<<recombine, longBlocks);
718          if (lowband)
719             deinterleave_hadamard(lowband, N_B>>recombine, B0<<recombine, longBlocks);
720       }
721    }
722
723    /* If we need more than 32 bits, try splitting the band in two. */
724    if (!stereo && LM != -1 && b > 32<<BITRES && N>2)
725    {
726       if (LM>0 || (N&1)==0)
727       {
728          N >>= 1;
729          Y = X+N;
730          split = 1;
731          LM -= 1;
732          if (B==1)
733             fill |= fill<<1;
734          B = (B+1)>>1;
735       }
736    }
737
738    if (split)
739    {
740       int qn;
741       int itheta=0;
742       int mbits, sbits, delta;
743       int qalloc;
744       int offset;
745       celt_int32 tell;
746
747       /* Decide on the resolution to give to the split parameter theta */
748       offset = ((m->logN[i]+(LM<<BITRES))>>1) - (stereo ? QTHETA_OFFSET_STEREO : QTHETA_OFFSET);
749       qn = compute_qn(N, b, offset, stereo);
750       if (stereo && i>=intensity)
751          qn = 1;
752       if (encode)
753       {
754          /* theta is the atan() of the ratio between the (normalized)
755             side and mid. With just that parameter, we can re-scale both
756             mid and side because we know that 1) they have unit norm and
757             2) they are orthogonal. */
758          itheta = stereo_itheta(X, Y, stereo, N);
759       }
760       tell = encode ? ec_enc_tell(ec, BITRES) : ec_dec_tell(ec, BITRES);
761       if (qn!=1)
762       {
763          if (encode)
764             itheta = (itheta*qn+8192)>>14;
765
766          /* Entropy coding of the angle. We use a uniform pdf for the
767             time split, a step for stereo, and a triangular one for the rest. */
768          if (stereo && N>2)
769          {
770             int p0 = 3;
771             int x = itheta;
772             int x0 = qn/2;
773             int ft = p0*(x0+1) + x0;
774             /* Use a probability of p0 up to itheta=8192 and then use 1 after */
775             if (encode)
776             {
777                ec_encode((ec_enc*)ec,x<=x0?p0*x:(x-1-x0)+(x0+1)*p0,x<=x0?p0*(x+1):(x-x0)+(x0+1)*p0,ft);
778             } else {
779                int fs;
780                fs=ec_decode(ec,ft);
781                if (fs<(x0+1)*p0)
782                   x=fs/p0;
783                else
784                   x=x0+1+(fs-(x0+1)*p0);
785                ec_dec_update(ec,x<=x0?p0*x:(x-1-x0)+(x0+1)*p0,x<=x0?p0*(x+1):(x-x0)+(x0+1)*p0,ft);
786                itheta = x;
787             }
788          } else if (B0>1 || stereo) {
789             /* Uniform pdf */
790             if (encode)
791                ec_enc_uint((ec_enc*)ec, itheta, qn+1);
792             else
793                itheta = ec_dec_uint((ec_dec*)ec, qn+1);
794          } else {
795             int fs=1, ft;
796             ft = ((qn>>1)+1)*((qn>>1)+1);
797             if (encode)
798             {
799                int fl;
800
801                fs = itheta <= (qn>>1) ? itheta + 1 : qn + 1 - itheta;
802                fl = itheta <= (qn>>1) ? itheta*(itheta + 1)>>1 :
803                 ft - ((qn + 1 - itheta)*(qn + 2 - itheta)>>1);
804
805                ec_encode((ec_enc*)ec, fl, fl+fs, ft);
806             } else {
807                /* Triangular pdf */
808                int fl=0;
809                int fm;
810                fm = ec_decode((ec_dec*)ec, ft);
811
812                if (fm < ((qn>>1)*((qn>>1) + 1)>>1))
813                {
814                   itheta = (isqrt32(8*(celt_uint32)fm + 1) - 1)>>1;
815                   fs = itheta + 1;
816                   fl = itheta*(itheta + 1)>>1;
817                }
818                else
819                {
820                   itheta = (2*(qn + 1)
821                    - isqrt32(8*(celt_uint32)(ft - fm - 1) + 1))>>1;
822                   fs = qn + 1 - itheta;
823                   fl = ft - ((qn + 1 - itheta)*(qn + 2 - itheta)>>1);
824                }
825
826                ec_dec_update((ec_dec*)ec, fl, fl+fs, ft);
827             }
828          }
829          itheta = (celt_int32)itheta*16384/qn;
830          if (encode && stereo)
831          {
832             if (itheta==0)
833                intensity_stereo(m, X, Y, bandE, i, N);
834             else
835                stereo_split(X, Y, N);
836          }
837          /* TODO: Renormalising X and Y *may* help fixed-point a bit at very high rate.
838                   Let's do that at higher complexity */
839       } else if (stereo) {
840          if (encode)
841          {
842             inv = itheta > 8192;
843             if (inv)
844             {
845                int j;
846                for (j=0;j<N;j++)
847                   Y[j] = -Y[j];
848             }
849             intensity_stereo(m, X, Y, bandE, i, N);
850          }
851          if (b>2<<BITRES && *remaining_bits > 2<<BITRES)
852          {
853             if (encode)
854                ec_enc_bit_logp(ec, inv, 2);
855             else
856                inv = ec_dec_bit_logp(ec, 2);
857          } else
858             inv = 0;
859          itheta = 0;
860       }
861       qalloc = (encode ? ec_enc_tell(ec, BITRES) : ec_dec_tell(ec, BITRES))
862                - tell;
863       b -= qalloc;
864
865       if (itheta == 0)
866       {
867          imid = 32767;
868          iside = 0;
869          fill &= (1<<B)-1;
870          delta = -16384;
871       } else if (itheta == 16384)
872       {
873          imid = 0;
874          iside = 32767;
875          fill &= (1<<B)-1<<B;
876          delta = 16384;
877       } else {
878          imid = bitexact_cos(itheta);
879          iside = bitexact_cos(16384-itheta);
880          /* This is the mid vs side allocation that minimizes squared error
881             in that band. */
882          delta = FRAC_MUL16(N-1<<7,bitexact_log2tan(iside,imid));
883       }
884
885 #ifdef FIXED_POINT
886       mid = imid;
887       side = iside;
888 #else
889       mid = (1.f/32768)*imid;
890       side = (1.f/32768)*iside;
891 #endif
892
893       /* This is a special case for N=2 that only works for stereo and takes
894          advantage of the fact that mid and side are orthogonal to encode
895          the side with just one bit. */
896       if (N==2 && stereo)
897       {
898          int c;
899          int sign=0;
900          celt_norm *x2, *y2;
901          mbits = b;
902          sbits = 0;
903          /* Only need one bit for the side */
904          if (itheta != 0 && itheta != 16384)
905             sbits = 1<<BITRES;
906          mbits -= sbits;
907          c = itheta > 8192;
908          *remaining_bits -= qalloc+sbits;
909
910          x2 = c ? Y : X;
911          y2 = c ? X : Y;
912          if (sbits)
913          {
914             if (encode)
915             {
916                /* Here we only need to encode a sign for the side */
917                sign = x2[0]*y2[1] - x2[1]*y2[0] < 0;
918                ec_enc_bits((ec_enc*)ec, sign, 1);
919             } else {
920                sign = ec_dec_bits((ec_dec*)ec, 1);
921             }
922          }
923          sign = 1-2*sign;
924          cm = quant_band(encode, m, i, x2, NULL, N, mbits, spread, B, intensity, tf_change, lowband, resynth, ec, remaining_bits, LM, lowband_out, NULL, level, seed, gain, lowband_scratch, fill);
925          /* We don't split N=2 bands, so cm is either 1 or 0 (for a fold-collapse),
926              and there's no need to worry about mixing with the other channel. */
927          y2[0] = -sign*x2[1];
928          y2[1] = sign*x2[0];
929          if (resynth)
930          {
931             celt_norm tmp;
932             X[0] = MULT16_16_Q15(mid, X[0]);
933             X[1] = MULT16_16_Q15(mid, X[1]);
934             Y[0] = MULT16_16_Q15(side, Y[0]);
935             Y[1] = MULT16_16_Q15(side, Y[1]);
936             tmp = X[0];
937             X[0] = SUB16(tmp,Y[0]);
938             Y[0] = ADD16(tmp,Y[0]);
939             tmp = X[1];
940             X[1] = SUB16(tmp,Y[1]);
941             Y[1] = ADD16(tmp,Y[1]);
942          }
943       } else {
944          /* "Normal" split code */
945          celt_norm *next_lowband2=NULL;
946          celt_norm *next_lowband_out1=NULL;
947          int next_level=0;
948
949          /* Give more bits to low-energy MDCTs than they would otherwise deserve */
950          if (B0>1 && !stereo)
951          {
952             if (itheta > 8192)
953                /* Rough approximation for pre-echo masking */
954                delta -= delta>>(4-LM);
955             else
956                /* Corresponds to a forward-masking slope of 1.5 dB per 10 ms */
957                delta = IMIN(0, delta + (N<<BITRES>>(5-LM)));
958          }
959          mbits = IMAX(0, IMIN(b, (b-delta)/2));
960          sbits = b-mbits;
961          *remaining_bits -= qalloc;
962
963          if (lowband && !stereo)
964             next_lowband2 = lowband+N; /* >32-bit split case */
965
966          /* Only stereo needs to pass on lowband_out. Otherwise, it's
967             handled at the end */
968          if (stereo)
969             next_lowband_out1 = lowband_out;
970          else
971             next_level = level+1;
972
973          /* In stereo mode, we do not apply a scaling to the mid because we need the normalized
974             mid for folding later */
975          cm = quant_band(encode, m, i, X, NULL, N, mbits, spread, B, intensity, tf_change,
976                lowband, resynth, ec, remaining_bits, LM, next_lowband_out1,
977                NULL, next_level, seed, stereo ? Q15ONE : MULT16_16_P15(gain,mid), lowband_scratch, fill);
978          /* For a stereo split, the high bits of fill are always zero, so no
979              folding will be done to the side. */
980          cm |= quant_band(encode, m, i, Y, NULL, N, sbits, spread, B, intensity, tf_change,
981                next_lowband2, resynth, ec, remaining_bits, LM, NULL,
982                NULL, next_level, seed, MULT16_16_P15(gain,side), NULL, fill>>B)<<B;
983       }
984
985    } else {
986       /* This is the basic no-split case */
987       q = bits2pulses(m, i, LM, b);
988       curr_bits = pulses2bits(m, i, LM, q);
989       *remaining_bits -= curr_bits;
990
991       /* Ensures we can never bust the budget */
992       while (*remaining_bits < 0 && q > 0)
993       {
994          *remaining_bits += curr_bits;
995          q--;
996          curr_bits = pulses2bits(m, i, LM, q);
997          *remaining_bits -= curr_bits;
998       }
999
1000       if (q!=0)
1001       {
1002          int K = get_pulses(q);
1003
1004          /* Finally do the actual quantization */
1005          if (encode)
1006             cm = alg_quant(X, N, K, spread, B, resynth, (ec_enc*)ec, gain);
1007          else
1008             cm = alg_unquant(X, N, K, spread, B, (ec_dec*)ec, gain);
1009       } else {
1010          /* If there's no pulse, fill the band anyway */
1011          int j;
1012          if (resynth)
1013          {
1014             if (!fill)
1015             {
1016                for (j=0;j<N;j++)
1017                   X[j] = 0;
1018             } else {
1019                if (lowband == NULL || (spread==SPREAD_AGGRESSIVE && B<=1))
1020                {
1021                   /* Noise */
1022                   for (j=0;j<N;j++)
1023                   {
1024                      *seed = lcg_rand(*seed);
1025                      X[j] = (celt_int32)(*seed)>>20;
1026                   }
1027                   cm = (1<<B)-1;
1028                } else {
1029                   /* Folded spectrum */
1030                   for (j=0;j<N;j++)
1031                      X[j] = lowband[j];
1032                   cm = fill;
1033                }
1034                renormalise_vector(X, N, gain);
1035             }
1036          }
1037       }
1038    }
1039
1040    /* This code is used by the decoder and by the resynthesis-enabled encoder */
1041    if (resynth)
1042    {
1043       if (stereo)
1044       {
1045          if (N!=2)
1046          {
1047             cm |= cm>>B;
1048             stereo_merge(X, Y, mid, N);
1049          }
1050          if (inv)
1051          {
1052             int j;
1053             for (j=0;j<N;j++)
1054                Y[j] = -Y[j];
1055          }
1056       } else if (level == 0)
1057       {
1058          int k;
1059
1060          /* Undo the sample reorganization going from time order to frequency order */
1061          if (B0>1)
1062             interleave_hadamard(X, N_B>>recombine, B0<<recombine, longBlocks);
1063
1064          /* Undo time-freq changes that we did earlier */
1065          N_B = N_B0;
1066          B = B0;
1067          for (k=0;k<time_divide;k++)
1068          {
1069             B >>= 1;
1070             N_B <<= 1;
1071             cm |= cm>>B;
1072             haar1(X, N_B, B);
1073          }
1074
1075          for (k=0;k<recombine;k++)
1076          {
1077             cm |= cm<<(1<<k);
1078             haar1(X, N0>>k, 1<<k);
1079          }
1080          B<<=recombine;
1081          N_B>>=recombine;
1082
1083          /* Scale output for later folding */
1084          if (lowband_out)
1085          {
1086             int j;
1087             celt_word16 n;
1088             n = celt_sqrt(SHL32(EXTEND32(N0),22));
1089             for (j=0;j<N0;j++)
1090                lowband_out[j] = MULT16_16_Q15(n,X[j]);
1091          }
1092       }
1093    }
1094    return cm;
1095 }
1096
1097 void quant_all_bands(int encode, const CELTMode *m, int start, int end,
1098       celt_norm *_X, celt_norm *_Y, unsigned char *collapse_masks, const celt_ener *bandE, int *pulses,
1099       int shortBlocks, int spread, int dual_stereo, int intensity, int *tf_res, int resynth,
1100       int total_bits, void *ec, int LM, int codedBands, ec_uint32 *seed)
1101 {
1102    int i;
1103    celt_int32 balance;
1104    celt_int32 remaining_bits;
1105    const celt_int16 * restrict eBands = m->eBands;
1106    celt_norm * restrict norm, * restrict norm2;
1107    VARDECL(celt_norm, _norm);
1108    VARDECL(celt_norm, lowband_scratch);
1109    int B;
1110    int M;
1111    int lowband_offset;
1112    int update_lowband = 1;
1113    int C = _Y != NULL ? 2 : 1;
1114    SAVE_STACK;
1115
1116    M = 1<<LM;
1117    B = shortBlocks ? M : 1;
1118    ALLOC(_norm, C*M*eBands[m->nbEBands], celt_norm);
1119    ALLOC(lowband_scratch, M*(eBands[m->nbEBands]-eBands[m->nbEBands-1]), celt_norm);
1120    norm = _norm;
1121    norm2 = norm + M*eBands[m->nbEBands];
1122
1123    balance = 0;
1124    lowband_offset = 0;
1125    for (i=start;i<end;i++)
1126    {
1127       celt_int32 tell;
1128       int b;
1129       int N;
1130       celt_int32 curr_balance;
1131       int effective_lowband=-1;
1132       celt_norm * restrict X, * restrict Y;
1133       int tf_change=0;
1134       unsigned x_cm;
1135       unsigned y_cm;
1136
1137       X = _X+M*eBands[i];
1138       if (_Y!=NULL)
1139          Y = _Y+M*eBands[i];
1140       else
1141          Y = NULL;
1142       N = M*eBands[i+1]-M*eBands[i];
1143       if (encode)
1144          tell = ec_enc_tell((ec_enc*)ec, BITRES);
1145       else
1146          tell = ec_dec_tell((ec_dec*)ec, BITRES);
1147
1148       /* Compute how many bits we want to allocate to this band */
1149       if (i != start)
1150          balance -= tell;
1151       remaining_bits = ((celt_int32)total_bits<<BITRES)-tell-1- (shortBlocks&&LM>=2 ? (1<<BITRES) : 0);
1152       if (i <= codedBands-1)
1153       {
1154          curr_balance = balance / IMIN(3, codedBands-i);
1155          b = IMAX(0, IMIN(16384, IMIN(remaining_bits+1,pulses[i]+curr_balance)));
1156       } else {
1157          b = 0;
1158       }
1159
1160       if (resynth && M*eBands[i]-N >= M*eBands[start] && (update_lowband || lowband_offset==0))
1161             lowband_offset = i;
1162
1163       tf_change = tf_res[i];
1164       if (i>=m->effEBands)
1165       {
1166          X=norm;
1167          if (_Y!=NULL)
1168             Y = norm;
1169       }
1170
1171       /* This ensures we never repeat spectral content within one band */
1172       if (lowband_offset != 0)
1173          effective_lowband = IMAX(M*eBands[start], M*eBands[lowband_offset]-N);
1174
1175       /* Get a conservative estimate of the collapse_mask's for the bands we're
1176           going to be folding from. */
1177       if (lowband_offset != 0 && (spread!=SPREAD_AGGRESSIVE || B>1))
1178       {
1179          int fold_start;
1180          int fold_end;
1181          int fold_i;
1182          fold_start = lowband_offset;
1183          while(M*eBands[--fold_start] > effective_lowband);
1184          fold_end = lowband_offset-1;
1185          while(M*eBands[++fold_end] < effective_lowband+N);
1186          x_cm = y_cm = 0;
1187          fold_i = fold_start; do {
1188            x_cm |= collapse_masks[fold_i*C+0];
1189            y_cm |= collapse_masks[fold_i*C+C-1];
1190          } while (++fold_i<fold_end);
1191       }
1192       /* Otherwise, we'll be using the LCG to fold, so all blocks will (almost
1193           always) be non-zero.*/
1194       else
1195          x_cm = y_cm = (1<<B)-1;
1196
1197       if (dual_stereo && i==intensity)
1198       {
1199          int j;
1200
1201          /* Switch off dual stereo to do intensity */
1202          dual_stereo = 0;
1203          for (j=M*eBands[start];j<M*eBands[i];j++)
1204             norm[j] = HALF32(norm[j]+norm2[j]);
1205       }
1206       if (dual_stereo)
1207       {
1208          x_cm = quant_band(encode, m, i, X, NULL, N, b/2, spread, B, intensity, tf_change,
1209                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1210                norm+M*eBands[i], bandE, 0, seed, Q15ONE, lowband_scratch, x_cm);
1211          y_cm = quant_band(encode, m, i, Y, NULL, N, b/2, spread, B, intensity, tf_change,
1212                effective_lowband != -1 ? norm2+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1213                norm2+M*eBands[i], bandE, 0, seed, Q15ONE, lowband_scratch, y_cm);
1214       } else {
1215          x_cm = quant_band(encode, m, i, X, Y, N, b, spread, B, intensity, tf_change,
1216                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1217                norm+M*eBands[i], bandE, 0, seed, Q15ONE, lowband_scratch, x_cm|y_cm);
1218          y_cm = x_cm;
1219       }
1220       collapse_masks[i*C+0] = (unsigned char)(x_cm&(1<<B)-1);
1221       collapse_masks[i*C+C-1] = (unsigned char)(y_cm&(1<<B)-1);
1222       balance += pulses[i] + tell;
1223
1224       /* Update the folding position only as long as we have 1 bit/sample depth */
1225       update_lowband = (b>>BITRES)>N;
1226    }
1227    RESTORE_STACK;
1228 }
1229