Don't rebalance bits for itheta=0 or 16384
[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 in 1/8 bits */
231       depth = (1+pulses[i])/(m->eBands[i+1]-m->eBands[i]<<LM);
232
233 #ifdef FIXED_POINT
234       thresh = MULT16_32_Q15(QCONST16(0.5f, 15), MIN32(32767,SHR32(celt_exp2(-SHL16(depth, 10-BITRES)),1) ));
235       {
236          celt_word32 t;
237          t = N0<<LM;
238          shift = celt_ilog2(t)>>1;
239          t = SHL32(t, (7-shift)<<1);
240          sqrt_1 = celt_rsqrt_norm(t);
241       }
242 #else
243       thresh = .5f*celt_exp2(-.125f*depth);
244       sqrt_1 = celt_rsqrt(N0<<LM);
245 #endif
246
247       c=0; do
248       {
249          celt_norm *X;
250          celt_word16 Ediff;
251          celt_word16 r;
252          Ediff = logE[c*m->nbEBands+i]-MIN16(prev1logE[c*m->nbEBands+i],prev2logE[c*m->nbEBands+i]);
253          Ediff = MAX16(0, Ediff);
254
255 #ifdef FIXED_POINT
256          if (Ediff < 16384)
257             r = 2*MIN16(16383,SHR32(celt_exp2(-Ediff),1));
258          else
259             r = 0;
260          if (LM==3)
261             r = MULT16_16_Q14(23170, MIN32(23169, r));
262          r = SHR16(MIN16(thresh, r),1);
263          r = SHR32(MULT16_16_Q15(sqrt_1, r),shift);
264 #else
265          /* r needs to be multiplied by 2 or 2*sqrt(2) depending on LM because
266             short blocks don't have the same energy as long */
267          r = 2.f*celt_exp2(-Ediff);
268          if (LM==3)
269             r *= 1.41421356f;
270          r = MIN16(thresh, r);
271          r = r*sqrt_1;
272 #endif
273          X = _X+c*size+(m->eBands[i]<<LM);
274          for (k=0;k<1<<LM;k++)
275          {
276             /* Detect collapse */
277             if (!(collapse_masks[i*C+c]&1<<k))
278             {
279                /* Fill with noise */
280                for (j=0;j<N0;j++)
281                {
282                   seed = lcg_rand(seed);
283                   X[(j<<LM)+k] = (seed&0x8000 ? r : -r);
284                }
285             }
286          }
287          /* We just added some energy, so we need to renormalise */
288          renormalise_vector(X, N0<<LM, Q15ONE);
289       } while (++c<C);
290    }
291
292 }
293
294
295 static void intensity_stereo(const CELTMode *m, celt_norm *X, celt_norm *Y, const celt_ener *bank, int bandID, int N)
296 {
297    int i = bandID;
298    int j;
299    celt_word16 a1, a2;
300    celt_word16 left, right;
301    celt_word16 norm;
302 #ifdef FIXED_POINT
303    int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
304 #endif
305    left = VSHR32(bank[i],shift);
306    right = VSHR32(bank[i+m->nbEBands],shift);
307    norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
308    a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
309    a2 = DIV32_16(SHL32(EXTEND32(right),14),norm);
310    for (j=0;j<N;j++)
311    {
312       celt_norm r, l;
313       l = X[j];
314       r = Y[j];
315       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
316       /* Side is not encoded, no need to calculate */
317    }
318 }
319
320 static void stereo_split(celt_norm *X, celt_norm *Y, int N)
321 {
322    int j;
323    for (j=0;j<N;j++)
324    {
325       celt_norm r, l;
326       l = MULT16_16_Q15(QCONST16(.70710678f,15), X[j]);
327       r = MULT16_16_Q15(QCONST16(.70710678f,15), Y[j]);
328       X[j] = l+r;
329       Y[j] = r-l;
330    }
331 }
332
333 static void stereo_merge(celt_norm *X, celt_norm *Y, celt_word16 mid, int N)
334 {
335    int j;
336    celt_word32 xp=0, side=0;
337    celt_word32 El, Er;
338    celt_word16 mid2;
339 #ifdef FIXED_POINT
340    int kl, kr;
341 #endif
342    celt_word32 t, lgain, rgain;
343
344    /* Compute the norm of X+Y and X-Y as |X|^2 + |Y|^2 +/- sum(xy) */
345    for (j=0;j<N;j++)
346    {
347       xp = MAC16_16(xp, X[j], Y[j]);
348       side = MAC16_16(side, Y[j], Y[j]);
349    }
350    /* Compensating for the mid normalization */
351    xp = MULT16_32_Q15(mid, xp);
352    /* mid and side are in Q15, not Q14 like X and Y */
353    mid2 = SHR32(mid, 1);
354    El = MULT16_16(mid2, mid2) + side - 2*xp;
355    Er = MULT16_16(mid2, mid2) + side + 2*xp;
356    if (Er < QCONST32(6e-4f, 28) || El < QCONST32(6e-4f, 28))
357    {
358       for (j=0;j<N;j++)
359          Y[j] = X[j];
360       return;
361    }
362
363 #ifdef FIXED_POINT
364    kl = celt_ilog2(El)>>1;
365    kr = celt_ilog2(Er)>>1;
366 #endif
367    t = VSHR32(El, (kl-7)<<1);
368    lgain = celt_rsqrt_norm(t);
369    t = VSHR32(Er, (kr-7)<<1);
370    rgain = celt_rsqrt_norm(t);
371
372 #ifdef FIXED_POINT
373    if (kl < 7)
374       kl = 7;
375    if (kr < 7)
376       kr = 7;
377 #endif
378
379    for (j=0;j<N;j++)
380    {
381       celt_norm r, l;
382       /* Apply mid scaling (side is already scaled) */
383       l = MULT16_16_Q15(mid, X[j]);
384       r = Y[j];
385       X[j] = EXTRACT16(PSHR32(MULT16_16(lgain, SUB16(l,r)), kl+1));
386       Y[j] = EXTRACT16(PSHR32(MULT16_16(rgain, ADD16(l,r)), kr+1));
387    }
388 }
389
390 /* Decide whether we should spread the pulses in the current frame */
391 int spreading_decision(const CELTMode *m, celt_norm *X, int *average,
392       int last_decision, int *hf_average, int *tapset_decision, int update_hf,
393       int end, int _C, int M)
394 {
395    int i, c, N0;
396    int sum = 0, nbBands=0;
397    const int C = CHANNELS(_C);
398    const celt_int16 * restrict eBands = m->eBands;
399    int decision;
400    int hf_sum=0;
401    
402    N0 = M*m->shortMdctSize;
403
404    if (M*(eBands[end]-eBands[end-1]) <= 8)
405       return SPREAD_NONE;
406    c=0; do {
407       for (i=0;i<end;i++)
408       {
409          int j, N, tmp=0;
410          int tcount[3] = {0};
411          celt_norm * restrict x = X+M*eBands[i]+c*N0;
412          N = M*(eBands[i+1]-eBands[i]);
413          if (N<=8)
414             continue;
415          /* Compute rough CDF of |x[j]| */
416          for (j=0;j<N;j++)
417          {
418             celt_word32 x2N; /* Q13 */
419
420             x2N = MULT16_16(MULT16_16_Q15(x[j], x[j]), N);
421             if (x2N < QCONST16(0.25f,13))
422                tcount[0]++;
423             if (x2N < QCONST16(0.0625f,13))
424                tcount[1]++;
425             if (x2N < QCONST16(0.015625f,13))
426                tcount[2]++;
427          }
428
429          /* Only include four last bands (8 kHz and up) */
430          if (i>m->nbEBands-4)
431             hf_sum += 32*(tcount[1]+tcount[0])/N;
432          tmp = (2*tcount[2] >= N) + (2*tcount[1] >= N) + (2*tcount[0] >= N);
433          sum += tmp*256;
434          nbBands++;
435       }
436    } while (++c<C);
437
438    if (update_hf)
439    {
440       if (hf_sum)
441          hf_sum /= C*(4-m->nbEBands+end);
442       *hf_average = (*hf_average+hf_sum)>>1;
443       hf_sum = *hf_average;
444       if (*tapset_decision==2)
445          hf_sum += 4;
446       else if (*tapset_decision==0)
447          hf_sum -= 4;
448       if (hf_sum > 22)
449          *tapset_decision=2;
450       else if (hf_sum > 18)
451          *tapset_decision=1;
452       else
453          *tapset_decision=0;
454    }
455    /*printf("%d %d %d\n", hf_sum, *hf_average, *tapset_decision);*/
456    sum /= nbBands;
457    /* Recursive averaging */
458    sum = (sum+*average)>>1;
459    *average = sum;
460    /* Hysteresis */
461    sum = (3*sum + (((3-last_decision)<<7) + 64) + 2)>>2;
462    if (sum < 80)
463    {
464       decision = SPREAD_AGGRESSIVE;
465    } else if (sum < 256)
466    {
467       decision = SPREAD_NORMAL;
468    } else if (sum < 384)
469    {
470       decision = SPREAD_LIGHT;
471    } else {
472       decision = SPREAD_NONE;
473    }
474    return decision;
475 }
476
477 #ifdef MEASURE_NORM_MSE
478
479 float MSE[30] = {0};
480 int nbMSEBands = 0;
481 int MSECount[30] = {0};
482
483 void dump_norm_mse(void)
484 {
485    int i;
486    for (i=0;i<nbMSEBands;i++)
487    {
488       printf ("%g ", MSE[i]/MSECount[i]);
489    }
490    printf ("\n");
491 }
492
493 void measure_norm_mse(const CELTMode *m, float *X, float *X0, float *bandE, float *bandE0, int M, int N, int C)
494 {
495    static int init = 0;
496    int i;
497    if (!init)
498    {
499       atexit(dump_norm_mse);
500       init = 1;
501    }
502    for (i=0;i<m->nbEBands;i++)
503    {
504       int j;
505       int c;
506       float g;
507       if (bandE0[i]<10 || (C==2 && bandE0[i+m->nbEBands]<1))
508          continue;
509       c=0; do {
510          g = bandE[i+c*m->nbEBands]/(1e-15+bandE0[i+c*m->nbEBands]);
511          for (j=M*m->eBands[i];j<M*m->eBands[i+1];j++)
512             MSE[i] += (g*X[j+c*N]-X0[j+c*N])*(g*X[j+c*N]-X0[j+c*N]);
513       } while (++c<C);
514       MSECount[i]+=C;
515    }
516    nbMSEBands = m->nbEBands;
517 }
518
519 #endif
520
521 /* Indexing table for converting from natural Hadamard to ordery Hadamard
522    This is essentially a bit-reversed Gray, on top of which we've added
523    an inversion of the order because we want the DC at the end rather than
524    the beginning. The lines are for N=2, 4, 8, 16 */
525 static const int ordery_table[] = {
526        1,  0,
527        3,  0,  2,  1,
528        7,  0,  4,  3,  6,  1,  5,  2,
529       15,  0,  8,  7, 12,  3, 11,  4, 14,  1,  9,  6, 13,  2, 10,  5,
530 };
531
532 static void deinterleave_hadamard(celt_norm *X, int N0, int stride, int hadamard)
533 {
534    int i,j;
535    VARDECL(celt_norm, tmp);
536    int N;
537    SAVE_STACK;
538    N = N0*stride;
539    ALLOC(tmp, N, celt_norm);
540    if (hadamard)
541    {
542       const int *ordery = ordery_table+stride-2;
543       for (i=0;i<stride;i++)
544       {
545          for (j=0;j<N0;j++)
546             tmp[ordery[i]*N0+j] = X[j*stride+i];
547       }
548    } else {
549       for (i=0;i<stride;i++)
550          for (j=0;j<N0;j++)
551             tmp[i*N0+j] = X[j*stride+i];
552    }
553    for (j=0;j<N;j++)
554       X[j] = tmp[j];
555    RESTORE_STACK;
556 }
557
558 static void interleave_hadamard(celt_norm *X, int N0, int stride, int hadamard)
559 {
560    int i,j;
561    VARDECL(celt_norm, tmp);
562    int N;
563    SAVE_STACK;
564    N = N0*stride;
565    ALLOC(tmp, N, celt_norm);
566    if (hadamard)
567    {
568       const int *ordery = ordery_table+stride-2;
569       for (i=0;i<stride;i++)
570          for (j=0;j<N0;j++)
571             tmp[j*stride+i] = X[ordery[i]*N0+j];
572    } else {
573       for (i=0;i<stride;i++)
574          for (j=0;j<N0;j++)
575             tmp[j*stride+i] = X[i*N0+j];
576    }
577    for (j=0;j<N;j++)
578       X[j] = tmp[j];
579    RESTORE_STACK;
580 }
581
582 void haar1(celt_norm *X, int N0, int stride)
583 {
584    int i, j;
585    N0 >>= 1;
586    for (i=0;i<stride;i++)
587       for (j=0;j<N0;j++)
588       {
589          celt_norm tmp1, tmp2;
590          tmp1 = MULT16_16_Q15(QCONST16(.70710678f,15), X[stride*2*j+i]);
591          tmp2 = MULT16_16_Q15(QCONST16(.70710678f,15), X[stride*(2*j+1)+i]);
592          X[stride*2*j+i] = tmp1 + tmp2;
593          X[stride*(2*j+1)+i] = tmp1 - tmp2;
594       }
595 }
596
597 static int compute_qn(int N, int b, int offset, int stereo)
598 {
599    static const celt_int16 exp2_table8[8] =
600       {16384, 17866, 19483, 21247, 23170, 25267, 27554, 30048};
601    int qn, qb;
602    int N2 = 2*N-1;
603    if (stereo && N==2)
604       N2--;
605    qb = IMIN((b>>1)-(1<<BITRES), (b+N2*offset)/N2);
606
607    qb = IMAX(0, IMIN(8<<BITRES, qb));
608
609    if (qb<(1<<BITRES>>1)) {
610       qn = 1;
611    } else {
612       qn = exp2_table8[qb&0x7]>>(14-(qb>>BITRES));
613       qn = (qn+1)>>1<<1;
614    }
615    celt_assert(qn <= 256);
616    return qn;
617 }
618
619 /* This function is responsible for encoding and decoding a band for both
620    the mono and stereo case. Even in the mono case, it can split the band
621    in two and transmit the energy difference with the two half-bands. It
622    can be called recursively so bands can end up being split in 8 parts. */
623 static unsigned quant_band(int encode, const CELTMode *m, int i, celt_norm *X, celt_norm *Y,
624       int N, int b, int spread, int B, int intensity, int tf_change, celt_norm *lowband, int resynth, void *ec,
625       celt_int32 *remaining_bits, int LM, celt_norm *lowband_out, const celt_ener *bandE, int level,
626       celt_uint32 *seed, celt_word16 gain, celt_norm *lowband_scratch, int fill)
627 {
628    int q;
629    int curr_bits;
630    int stereo, split;
631    int imid=0, iside=0;
632    int N0=N;
633    int N_B=N;
634    int N_B0;
635    int B0=B;
636    int time_divide=0;
637    int recombine=0;
638    int inv = 0;
639    celt_word16 mid=0, side=0;
640    int longBlocks;
641    unsigned cm=0;
642
643    longBlocks = B0==1;
644
645    N_B /= B;
646    N_B0 = N_B;
647
648    split = stereo = Y != NULL;
649
650    /* Special case for one sample */
651    if (N==1)
652    {
653       int c;
654       celt_norm *x = X;
655       c=0; do {
656          int sign=0;
657          if (*remaining_bits>=1<<BITRES)
658          {
659             if (encode)
660             {
661                sign = x[0]<0;
662                ec_enc_bits((ec_enc*)ec, sign, 1);
663             } else {
664                sign = ec_dec_bits((ec_dec*)ec, 1);
665             }
666             *remaining_bits -= 1<<BITRES;
667             b-=1<<BITRES;
668          }
669          if (resynth)
670             x[0] = sign ? -NORM_SCALING : NORM_SCALING;
671          x = Y;
672       } while (++c<1+stereo);
673       if (lowband_out)
674          lowband_out[0] = SHR16(X[0],4);
675       return 1;
676    }
677
678    if (!stereo && level == 0)
679    {
680       int k;
681       if (tf_change>0)
682          recombine = tf_change;
683       /* Band recombining to increase frequency resolution */
684
685       if (lowband && (recombine || ((N_B&1) == 0 && tf_change<0) || B0>1))
686       {
687          int j;
688          for (j=0;j<N;j++)
689             lowband_scratch[j] = lowband[j];
690          lowband = lowband_scratch;
691       }
692
693       for (k=0;k<recombine;k++)
694       {
695          if (encode)
696             haar1(X, N>>k, 1<<k);
697          if (lowband)
698             haar1(lowband, N>>k, 1<<k);
699          fill |= fill<<(1<<k);
700       }
701       B>>=recombine;
702       N_B<<=recombine;
703
704       /* Increasing the time resolution */
705       while ((N_B&1) == 0 && tf_change<0)
706       {
707          if (encode)
708             haar1(X, N_B, B);
709          if (lowband)
710             haar1(lowband, N_B, B);
711          fill |= fill<<B;
712          B <<= 1;
713          N_B >>= 1;
714          time_divide++;
715          tf_change++;
716       }
717       B0=B;
718       N_B0 = N_B;
719
720       /* Reorganize the samples in time order instead of frequency order */
721       if (B0>1)
722       {
723          if (encode)
724             deinterleave_hadamard(X, N_B>>recombine, B0<<recombine, longBlocks);
725          if (lowband)
726             deinterleave_hadamard(lowband, N_B>>recombine, B0<<recombine, longBlocks);
727       }
728    }
729
730    /* If we need more than 32 bits, try splitting the band in two. */
731    if (!stereo && LM != -1 && b > 32<<BITRES && N>2)
732    {
733       if (LM>0 || (N&1)==0)
734       {
735          N >>= 1;
736          Y = X+N;
737          split = 1;
738          LM -= 1;
739          if (B==1)
740             fill = fill&1|fill<<1;
741          B = (B+1)>>1;
742       }
743    }
744
745    if (split)
746    {
747       int qn;
748       int itheta=0;
749       int mbits, sbits, delta;
750       int qalloc;
751       int offset;
752       int orig_fill;
753       celt_int32 tell;
754
755       /* Decide on the resolution to give to the split parameter theta */
756       offset = ((m->logN[i]+(LM<<BITRES))>>1) - (stereo ? QTHETA_OFFSET_STEREO : QTHETA_OFFSET);
757       qn = compute_qn(N, b, offset, stereo);
758       if (stereo && i>=intensity)
759          qn = 1;
760       if (encode)
761       {
762          /* theta is the atan() of the ratio between the (normalized)
763             side and mid. With just that parameter, we can re-scale both
764             mid and side because we know that 1) they have unit norm and
765             2) they are orthogonal. */
766          itheta = stereo_itheta(X, Y, stereo, N);
767       }
768       tell = encode ? ec_enc_tell(ec, BITRES) : ec_dec_tell(ec, BITRES);
769       if (qn!=1)
770       {
771          if (encode)
772             itheta = (itheta*qn+8192)>>14;
773
774          /* Entropy coding of the angle. We use a uniform pdf for the
775             time split, a step for stereo, and a triangular one for the rest. */
776          if (stereo && N>2)
777          {
778             int p0 = 3;
779             int x = itheta;
780             int x0 = qn/2;
781             int ft = p0*(x0+1) + x0;
782             /* Use a probability of p0 up to itheta=8192 and then use 1 after */
783             if (encode)
784             {
785                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);
786             } else {
787                int fs;
788                fs=ec_decode(ec,ft);
789                if (fs<(x0+1)*p0)
790                   x=fs/p0;
791                else
792                   x=x0+1+(fs-(x0+1)*p0);
793                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);
794                itheta = x;
795             }
796          } else if (B0>1 || stereo) {
797             /* Uniform pdf */
798             if (encode)
799                ec_enc_uint((ec_enc*)ec, itheta, qn+1);
800             else
801                itheta = ec_dec_uint((ec_dec*)ec, qn+1);
802          } else {
803             int fs=1, ft;
804             ft = ((qn>>1)+1)*((qn>>1)+1);
805             if (encode)
806             {
807                int fl;
808
809                fs = itheta <= (qn>>1) ? itheta + 1 : qn + 1 - itheta;
810                fl = itheta <= (qn>>1) ? itheta*(itheta + 1)>>1 :
811                 ft - ((qn + 1 - itheta)*(qn + 2 - itheta)>>1);
812
813                ec_encode((ec_enc*)ec, fl, fl+fs, ft);
814             } else {
815                /* Triangular pdf */
816                int fl=0;
817                int fm;
818                fm = ec_decode((ec_dec*)ec, ft);
819
820                if (fm < ((qn>>1)*((qn>>1) + 1)>>1))
821                {
822                   itheta = (isqrt32(8*(celt_uint32)fm + 1) - 1)>>1;
823                   fs = itheta + 1;
824                   fl = itheta*(itheta + 1)>>1;
825                }
826                else
827                {
828                   itheta = (2*(qn + 1)
829                    - isqrt32(8*(celt_uint32)(ft - fm - 1) + 1))>>1;
830                   fs = qn + 1 - itheta;
831                   fl = ft - ((qn + 1 - itheta)*(qn + 2 - itheta)>>1);
832                }
833
834                ec_dec_update((ec_dec*)ec, fl, fl+fs, ft);
835             }
836          }
837          itheta = (celt_int32)itheta*16384/qn;
838          if (encode && stereo)
839          {
840             if (itheta==0)
841                intensity_stereo(m, X, Y, bandE, i, N);
842             else
843                stereo_split(X, Y, N);
844          }
845          /* TODO: Renormalising X and Y *may* help fixed-point a bit at very high rate.
846                   Let's do that at higher complexity */
847       } else if (stereo) {
848          if (encode)
849          {
850             inv = itheta > 8192;
851             if (inv)
852             {
853                int j;
854                for (j=0;j<N;j++)
855                   Y[j] = -Y[j];
856             }
857             intensity_stereo(m, X, Y, bandE, i, N);
858          }
859          if (b>2<<BITRES && *remaining_bits > 2<<BITRES)
860          {
861             if (encode)
862                ec_enc_bit_logp(ec, inv, 2);
863             else
864                inv = ec_dec_bit_logp(ec, 2);
865          } else
866             inv = 0;
867          itheta = 0;
868       }
869       qalloc = (encode ? ec_enc_tell(ec, BITRES) : ec_dec_tell(ec, BITRES))
870                - tell;
871       b -= qalloc;
872
873       orig_fill = fill;
874       if (itheta == 0)
875       {
876          imid = 32767;
877          iside = 0;
878          fill &= (1<<B)-1;
879          delta = -16384;
880       } else if (itheta == 16384)
881       {
882          imid = 0;
883          iside = 32767;
884          fill &= (1<<B)-1<<B;
885          delta = 16384;
886       } else {
887          imid = bitexact_cos(itheta);
888          iside = bitexact_cos(16384-itheta);
889          /* This is the mid vs side allocation that minimizes squared error
890             in that band. */
891          delta = FRAC_MUL16(N-1<<7,bitexact_log2tan(iside,imid));
892       }
893
894 #ifdef FIXED_POINT
895       mid = imid;
896       side = iside;
897 #else
898       mid = (1.f/32768)*imid;
899       side = (1.f/32768)*iside;
900 #endif
901
902       /* This is a special case for N=2 that only works for stereo and takes
903          advantage of the fact that mid and side are orthogonal to encode
904          the side with just one bit. */
905       if (N==2 && stereo)
906       {
907          int c;
908          int sign=0;
909          celt_norm *x2, *y2;
910          mbits = b;
911          sbits = 0;
912          /* Only need one bit for the side */
913          if (itheta != 0 && itheta != 16384)
914             sbits = 1<<BITRES;
915          mbits -= sbits;
916          c = itheta > 8192;
917          *remaining_bits -= qalloc+sbits;
918
919          x2 = c ? Y : X;
920          y2 = c ? X : Y;
921          if (sbits)
922          {
923             if (encode)
924             {
925                /* Here we only need to encode a sign for the side */
926                sign = x2[0]*y2[1] - x2[1]*y2[0] < 0;
927                ec_enc_bits((ec_enc*)ec, sign, 1);
928             } else {
929                sign = ec_dec_bits((ec_dec*)ec, 1);
930             }
931          }
932          sign = 1-2*sign;
933          /* We use orig_fill here because we want to fold the side, but if
934              itheta==16384, we'll have cleared the low bits of fill. */
935          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, orig_fill);
936          /* We don't split N=2 bands, so cm is either 1 or 0 (for a fold-collapse),
937              and there's no need to worry about mixing with the other channel. */
938          y2[0] = -sign*x2[1];
939          y2[1] = sign*x2[0];
940          if (resynth)
941          {
942             celt_norm tmp;
943             X[0] = MULT16_16_Q15(mid, X[0]);
944             X[1] = MULT16_16_Q15(mid, X[1]);
945             Y[0] = MULT16_16_Q15(side, Y[0]);
946             Y[1] = MULT16_16_Q15(side, Y[1]);
947             tmp = X[0];
948             X[0] = SUB16(tmp,Y[0]);
949             Y[0] = ADD16(tmp,Y[0]);
950             tmp = X[1];
951             X[1] = SUB16(tmp,Y[1]);
952             Y[1] = ADD16(tmp,Y[1]);
953          }
954       } else {
955          /* "Normal" split code */
956          celt_norm *next_lowband2=NULL;
957          celt_norm *next_lowband_out1=NULL;
958          int next_level=0;
959          celt_int32 rebalance;
960
961          /* Give more bits to low-energy MDCTs than they would otherwise deserve */
962          if (B0>1 && !stereo)
963          {
964             if (itheta > 8192)
965                /* Rough approximation for pre-echo masking */
966                delta -= delta>>(4-LM);
967             else
968                /* Corresponds to a forward-masking slope of 1.5 dB per 10 ms */
969                delta = IMIN(0, delta + (N<<BITRES>>(5-LM)));
970          }
971          mbits = IMAX(0, IMIN(b, (b-delta)/2));
972          sbits = b-mbits;
973          *remaining_bits -= qalloc;
974
975          if (lowband && !stereo)
976             next_lowband2 = lowband+N; /* >32-bit split case */
977
978          /* Only stereo needs to pass on lowband_out. Otherwise, it's
979             handled at the end */
980          if (stereo)
981             next_lowband_out1 = lowband_out;
982          else
983             next_level = level+1;
984
985          rebalance = *remaining_bits;
986          if (mbits >= sbits)
987          {
988             /* In stereo mode, we do not apply a scaling to the mid because we need the normalized
989                mid for folding later */
990             cm = quant_band(encode, m, i, X, NULL, N, mbits, spread, B, intensity, tf_change,
991                   lowband, resynth, ec, remaining_bits, LM, next_lowband_out1,
992                   NULL, next_level, seed, stereo ? Q15ONE : MULT16_16_P15(gain,mid), lowband_scratch, fill);
993             rebalance = mbits - (rebalance-*remaining_bits);
994             if (rebalance > 3<<BITRES && itheta!=0)
995                sbits += rebalance - (3<<BITRES);
996
997             /* For a stereo split, the high bits of fill are always zero, so no
998                folding will be done to the side. */
999             cm |= quant_band(encode, m, i, Y, NULL, N, sbits, spread, B, intensity, tf_change,
1000                   next_lowband2, resynth, ec, remaining_bits, LM, NULL,
1001                   NULL, next_level, seed, MULT16_16_P15(gain,side), NULL, fill>>B)<<(B0>>1&stereo-1);
1002          } else {
1003             /* For a stereo split, the high bits of fill are always zero, so no
1004                folding will be done to the side. */
1005             cm = quant_band(encode, m, i, Y, NULL, N, sbits, spread, B, intensity, tf_change,
1006                   next_lowband2, resynth, ec, remaining_bits, LM, NULL,
1007                   NULL, next_level, seed, MULT16_16_P15(gain,side), NULL, fill>>B)<<(B0>>1&stereo-1);
1008             rebalance = sbits - (rebalance-*remaining_bits);
1009             if (rebalance > 3<<BITRES && itheta!=16384)
1010                mbits += rebalance - (3<<BITRES);
1011             /* In stereo mode, we do not apply a scaling to the mid because we need the normalized
1012                mid for folding later */
1013             cm |= quant_band(encode, m, i, X, NULL, N, mbits, spread, B, intensity, tf_change,
1014                   lowband, resynth, ec, remaining_bits, LM, next_lowband_out1,
1015                   NULL, next_level, seed, stereo ? Q15ONE : MULT16_16_P15(gain,mid), lowband_scratch, fill);
1016          }
1017       }
1018
1019    } else {
1020       /* This is the basic no-split case */
1021       q = bits2pulses(m, i, LM, b);
1022       curr_bits = pulses2bits(m, i, LM, q);
1023       *remaining_bits -= curr_bits;
1024
1025       /* Ensures we can never bust the budget */
1026       while (*remaining_bits < 0 && q > 0)
1027       {
1028          *remaining_bits += curr_bits;
1029          q--;
1030          curr_bits = pulses2bits(m, i, LM, q);
1031          *remaining_bits -= curr_bits;
1032       }
1033
1034       if (q!=0)
1035       {
1036          int K = get_pulses(q);
1037
1038          /* Finally do the actual quantization */
1039          if (encode)
1040             cm = alg_quant(X, N, K, spread, B, resynth, (ec_enc*)ec, gain);
1041          else
1042             cm = alg_unquant(X, N, K, spread, B, (ec_dec*)ec, gain);
1043       } else {
1044          /* If there's no pulse, fill the band anyway */
1045          int j;
1046          if (resynth)
1047          {
1048             unsigned cm_mask;
1049             /*B can be as large as 16, so this shift might overflow an int on a
1050                16-bit platform; use a long to get defined behavior.*/
1051             cm_mask = (unsigned)(1UL<<B)-1;
1052             fill &= cm_mask;
1053             if (!fill)
1054             {
1055                for (j=0;j<N;j++)
1056                   X[j] = 0;
1057             } else {
1058                if (lowband == NULL)
1059                {
1060                   /* Noise */
1061                   for (j=0;j<N;j++)
1062                   {
1063                      *seed = lcg_rand(*seed);
1064                      X[j] = (celt_int32)(*seed)>>20;
1065                   }
1066                   cm = cm_mask;
1067                } else {
1068                   /* Folded spectrum */
1069                   for (j=0;j<N;j++)
1070                      X[j] = lowband[j];
1071                   cm = fill;
1072                }
1073                renormalise_vector(X, N, gain);
1074             }
1075          }
1076       }
1077    }
1078
1079    /* This code is used by the decoder and by the resynthesis-enabled encoder */
1080    if (resynth)
1081    {
1082       if (stereo)
1083       {
1084          if (N!=2)
1085             stereo_merge(X, Y, mid, N);
1086          if (inv)
1087          {
1088             int j;
1089             for (j=0;j<N;j++)
1090                Y[j] = -Y[j];
1091          }
1092       } else if (level == 0)
1093       {
1094          int k;
1095
1096          /* Undo the sample reorganization going from time order to frequency order */
1097          if (B0>1)
1098             interleave_hadamard(X, N_B>>recombine, B0<<recombine, longBlocks);
1099
1100          /* Undo time-freq changes that we did earlier */
1101          N_B = N_B0;
1102          B = B0;
1103          for (k=0;k<time_divide;k++)
1104          {
1105             B >>= 1;
1106             N_B <<= 1;
1107             cm |= cm>>B;
1108             haar1(X, N_B, B);
1109          }
1110
1111          for (k=0;k<recombine;k++)
1112          {
1113             cm |= cm<<(1<<k);
1114             haar1(X, N0>>k, 1<<k);
1115          }
1116          B<<=recombine;
1117          N_B>>=recombine;
1118
1119          /* Scale output for later folding */
1120          if (lowband_out)
1121          {
1122             int j;
1123             celt_word16 n;
1124             n = celt_sqrt(SHL32(EXTEND32(N0),22));
1125             for (j=0;j<N0;j++)
1126                lowband_out[j] = MULT16_16_Q15(n,X[j]);
1127          }
1128          cm &= (1<<B)-1;
1129       }
1130    }
1131    return cm;
1132 }
1133
1134 void quant_all_bands(int encode, const CELTMode *m, int start, int end,
1135       celt_norm *_X, celt_norm *_Y, unsigned char *collapse_masks, const celt_ener *bandE, int *pulses,
1136       int shortBlocks, int spread, int dual_stereo, int intensity, int *tf_res, int resynth,
1137       celt_int32 total_bits, void *ec, int LM, int codedBands, ec_uint32 *seed)
1138 {
1139    int i;
1140    celt_int32 balance;
1141    celt_int32 remaining_bits;
1142    const celt_int16 * restrict eBands = m->eBands;
1143    celt_norm * restrict norm, * restrict norm2;
1144    VARDECL(celt_norm, _norm);
1145    VARDECL(celt_norm, lowband_scratch);
1146    int B;
1147    int M;
1148    int lowband_offset;
1149    int update_lowband = 1;
1150    int C = _Y != NULL ? 2 : 1;
1151    SAVE_STACK;
1152
1153    M = 1<<LM;
1154    B = shortBlocks ? M : 1;
1155    ALLOC(_norm, C*M*eBands[m->nbEBands], celt_norm);
1156    ALLOC(lowband_scratch, M*(eBands[m->nbEBands]-eBands[m->nbEBands-1]), celt_norm);
1157    norm = _norm;
1158    norm2 = norm + M*eBands[m->nbEBands];
1159
1160    balance = 0;
1161    lowband_offset = 0;
1162    for (i=start;i<end;i++)
1163    {
1164       celt_int32 tell;
1165       int b;
1166       int N;
1167       celt_int32 curr_balance;
1168       int effective_lowband=-1;
1169       celt_norm * restrict X, * restrict Y;
1170       int tf_change=0;
1171       unsigned x_cm;
1172       unsigned y_cm;
1173
1174       X = _X+M*eBands[i];
1175       if (_Y!=NULL)
1176          Y = _Y+M*eBands[i];
1177       else
1178          Y = NULL;
1179       N = M*eBands[i+1]-M*eBands[i];
1180       if (encode)
1181          tell = ec_enc_tell((ec_enc*)ec, BITRES);
1182       else
1183          tell = ec_dec_tell((ec_dec*)ec, BITRES);
1184
1185       /* Compute how many bits we want to allocate to this band */
1186       if (i != start)
1187          balance -= tell;
1188       remaining_bits = total_bits-tell-1;
1189       if (i <= codedBands-1)
1190       {
1191          curr_balance = balance / IMIN(3, codedBands-i);
1192          b = IMAX(0, IMIN(16384, IMIN(remaining_bits+1,pulses[i]+curr_balance)));
1193       } else {
1194          b = 0;
1195       }
1196
1197       if (resynth && M*eBands[i]-N >= M*eBands[start] && (update_lowband || lowband_offset==0))
1198             lowband_offset = i;
1199
1200       tf_change = tf_res[i];
1201       if (i>=m->effEBands)
1202       {
1203          X=norm;
1204          if (_Y!=NULL)
1205             Y = norm;
1206       }
1207
1208       /* Get a conservative estimate of the collapse_mask's for the bands we're
1209           going to be folding from. */
1210       if (lowband_offset != 0 && (spread!=SPREAD_AGGRESSIVE || B>1 || tf_change<0))
1211       {
1212          int fold_start;
1213          int fold_end;
1214          int fold_i;
1215          /* This ensures we never repeat spectral content within one band */
1216          effective_lowband = IMAX(M*eBands[start], M*eBands[lowband_offset]-N);
1217          fold_start = lowband_offset;
1218          while(M*eBands[--fold_start] > effective_lowband);
1219          fold_end = lowband_offset-1;
1220          while(M*eBands[++fold_end] < effective_lowband+N);
1221          x_cm = y_cm = 0;
1222          fold_i = fold_start; do {
1223            x_cm |= collapse_masks[fold_i*C+0];
1224            y_cm |= collapse_masks[fold_i*C+C-1];
1225          } while (++fold_i<fold_end);
1226       }
1227       /* Otherwise, we'll be using the LCG to fold, so all blocks will (almost
1228           always) be non-zero.*/
1229       else
1230          x_cm = y_cm = (1<<B)-1;
1231
1232       if (dual_stereo && i==intensity)
1233       {
1234          int j;
1235
1236          /* Switch off dual stereo to do intensity */
1237          dual_stereo = 0;
1238          for (j=M*eBands[start];j<M*eBands[i];j++)
1239             norm[j] = HALF32(norm[j]+norm2[j]);
1240       }
1241       if (dual_stereo)
1242       {
1243          x_cm = quant_band(encode, m, i, X, NULL, N, b/2, spread, B, intensity, tf_change,
1244                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1245                norm+M*eBands[i], bandE, 0, seed, Q15ONE, lowband_scratch, x_cm);
1246          y_cm = quant_band(encode, m, i, Y, NULL, N, b/2, spread, B, intensity, tf_change,
1247                effective_lowband != -1 ? norm2+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1248                norm2+M*eBands[i], bandE, 0, seed, Q15ONE, lowband_scratch, y_cm);
1249       } else {
1250          x_cm = quant_band(encode, m, i, X, Y, N, b, spread, B, intensity, tf_change,
1251                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1252                norm+M*eBands[i], bandE, 0, seed, Q15ONE, lowband_scratch, x_cm|y_cm);
1253          y_cm = x_cm;
1254       }
1255       collapse_masks[i*C+0] = (unsigned char)x_cm;
1256       collapse_masks[i*C+C-1] = (unsigned char)y_cm;
1257       balance += pulses[i] + tell;
1258
1259       /* Update the folding position only as long as we have 1 bit/sample depth */
1260       update_lowband = b>(N<<BITRES);
1261    }
1262    RESTORE_STACK;
1263 }
1264