Update the maximum fine bits everywhere.
[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 /* This is a cos() approximation designed to be bit-exact on any platform. Bit exactness
49    with this approximation is important because it has an impact on the bit allocation */
50 static celt_int16 bitexact_cos(celt_int16 x)
51 {
52    celt_int32 tmp;
53    celt_int16 x2;
54    tmp = (4096+((celt_int32)(x)*(x)))>>13;
55    if (tmp > 32767)
56       tmp = 32767;
57    x2 = tmp;
58    x2 = (32767-x2) + FRAC_MUL16(x2, (-7651 + FRAC_MUL16(x2, (8277 + FRAC_MUL16(-626, x2)))));
59    if (x2 > 32766)
60       x2 = 32766;
61    return 1+x2;
62 }
63
64
65 #ifdef FIXED_POINT
66 /* Compute the amplitude (sqrt energy) in each of the bands */
67 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int end, int _C, int M)
68 {
69    int i, c, N;
70    const celt_int16 *eBands = m->eBands;
71    const int C = CHANNELS(_C);
72    N = M*m->shortMdctSize;
73    c=0; do {
74       for (i=0;i<end;i++)
75       {
76          int j;
77          celt_word32 maxval=0;
78          celt_word32 sum = 0;
79          
80          j=M*eBands[i]; do {
81             maxval = MAX32(maxval, X[j+c*N]);
82             maxval = MAX32(maxval, -X[j+c*N]);
83          } while (++j<M*eBands[i+1]);
84          
85          if (maxval > 0)
86          {
87             int shift = celt_ilog2(maxval)-10;
88             j=M*eBands[i]; do {
89                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
90                                    EXTRACT16(VSHR32(X[j+c*N],shift)));
91             } while (++j<M*eBands[i+1]);
92             /* We're adding one here to make damn sure we never end up with a pitch vector that's
93                larger than unity norm */
94             bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
95          } else {
96             bank[i+c*m->nbEBands] = EPSILON;
97          }
98          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
99       }
100    } while (++c<C);
101    /*printf ("\n");*/
102 }
103
104 /* Normalise each band such that the energy is one. */
105 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)
106 {
107    int i, c, N;
108    const celt_int16 *eBands = m->eBands;
109    const int C = CHANNELS(_C);
110    N = M*m->shortMdctSize;
111    c=0; do {
112       i=0; do {
113          celt_word16 g;
114          int j,shift;
115          celt_word16 E;
116          shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
117          E = VSHR32(bank[i+c*m->nbEBands], shift);
118          g = EXTRACT16(celt_rcp(SHL32(E,3)));
119          j=M*eBands[i]; do {
120             X[j+c*N] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
121          } while (++j<M*eBands[i+1]);
122       } while (++i<end);
123    } while (++c<C);
124 }
125
126 #else /* FIXED_POINT */
127 /* Compute the amplitude (sqrt energy) in each of the bands */
128 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int end, int _C, int M)
129 {
130    int i, c, N;
131    const celt_int16 *eBands = m->eBands;
132    const int C = CHANNELS(_C);
133    N = M*m->shortMdctSize;
134    c=0; do {
135       for (i=0;i<end;i++)
136       {
137          int j;
138          celt_word32 sum = 1e-10f;
139          for (j=M*eBands[i];j<M*eBands[i+1];j++)
140             sum += X[j+c*N]*X[j+c*N];
141          bank[i+c*m->nbEBands] = celt_sqrt(sum);
142          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
143       }
144    } while (++c<C);
145    /*printf ("\n");*/
146 }
147
148 /* Normalise each band such that the energy is one. */
149 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)
150 {
151    int i, c, N;
152    const celt_int16 *eBands = m->eBands;
153    const int C = CHANNELS(_C);
154    N = M*m->shortMdctSize;
155    c=0; do {
156       for (i=0;i<end;i++)
157       {
158          int j;
159          celt_word16 g = 1.f/(1e-10f+bank[i+c*m->nbEBands]);
160          for (j=M*eBands[i];j<M*eBands[i+1];j++)
161             X[j+c*N] = freq[j+c*N]*g;
162       }
163    } while (++c<C);
164 }
165
166 #endif /* FIXED_POINT */
167
168 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
169 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)
170 {
171    int i, c, N;
172    const celt_int16 *eBands = m->eBands;
173    const int C = CHANNELS(_C);
174    N = M*m->shortMdctSize;
175    celt_assert2(C<=2, "denormalise_bands() not implemented for >2 channels");
176    c=0; do {
177       celt_sig * restrict f;
178       const celt_norm * restrict x;
179       f = freq+c*N;
180       x = X+c*N;
181       for (i=0;i<end;i++)
182       {
183          int j, band_end;
184          celt_word32 g = SHR32(bank[i+c*m->nbEBands],1);
185          j=M*eBands[i];
186          band_end = M*eBands[i+1];
187          do {
188             *f++ = SHL32(MULT16_32_Q15(*x, g),2);
189             x++;
190          } while (++j<band_end);
191       }
192       for (i=M*eBands[m->nbEBands];i<N;i++)
193          *f++ = 0;
194    } while (++c<C);
195 }
196
197 static void intensity_stereo(const CELTMode *m, celt_norm *X, celt_norm *Y, const celt_ener *bank, int bandID, int N)
198 {
199    int i = bandID;
200    int j;
201    celt_word16 a1, a2;
202    celt_word16 left, right;
203    celt_word16 norm;
204 #ifdef FIXED_POINT
205    int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
206 #endif
207    left = VSHR32(bank[i],shift);
208    right = VSHR32(bank[i+m->nbEBands],shift);
209    norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
210    a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
211    a2 = DIV32_16(SHL32(EXTEND32(right),14),norm);
212    for (j=0;j<N;j++)
213    {
214       celt_norm r, l;
215       l = X[j];
216       r = Y[j];
217       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
218       /* Side is not encoded, no need to calculate */
219    }
220 }
221
222 static void stereo_split(celt_norm *X, celt_norm *Y, int N)
223 {
224    int j;
225    for (j=0;j<N;j++)
226    {
227       celt_norm r, l;
228       l = MULT16_16_Q15(QCONST16(.70711f,15), X[j]);
229       r = MULT16_16_Q15(QCONST16(.70711f,15), Y[j]);
230       X[j] = l+r;
231       Y[j] = r-l;
232    }
233 }
234
235 static void stereo_merge(celt_norm *X, celt_norm *Y, celt_word16 mid, int N)
236 {
237    int j;
238    celt_word32 xp=0, side=0;
239    celt_word32 El, Er;
240 #ifdef FIXED_POINT
241    int kl, kr;
242 #endif
243    celt_word32 t, lgain, rgain;
244
245    /* Compute the norm of X+Y and X-Y as |X|^2 + |Y|^2 +/- sum(xy) */
246    for (j=0;j<N;j++)
247    {
248       xp = MAC16_16(xp, X[j], Y[j]);
249       side = MAC16_16(side, Y[j], Y[j]);
250    }
251    /* mid and side are in Q15, not Q14 like X and Y */
252    mid = SHR32(mid, 1);
253    //side = SHR32(side, 1);
254    El = MULT16_16(mid, mid) + side - 2*xp;
255    Er = MULT16_16(mid, mid) + side + 2*xp;
256    if (Er < EPSILON)
257       Er = EPSILON;
258    if (El < EPSILON)
259       El = EPSILON;
260
261 #ifdef FIXED_POINT
262    kl = celt_ilog2(El)>>1;
263    kr = celt_ilog2(Er)>>1;
264 #endif
265    t = VSHR32(El, (kl-7)<<1);
266    lgain = celt_rsqrt_norm(t);
267    t = VSHR32(Er, (kr-7)<<1);
268    rgain = celt_rsqrt_norm(t);
269
270 #ifdef FIXED_POINT
271    if (kl < 7)
272       kl = 7;
273    if (kr < 7)
274       kr = 7;
275 #endif
276
277    for (j=0;j<N;j++)
278    {
279       celt_norm r, l;
280       l = X[j];
281       r = Y[j];
282       X[j] = EXTRACT16(PSHR32(MULT16_16(lgain, SUB16(l,r)), kl+1));
283       Y[j] = EXTRACT16(PSHR32(MULT16_16(rgain, ADD16(l,r)), kr+1));
284    }
285 }
286
287 /* Decide whether we should spread the pulses in the current frame */
288 int spreading_decision(const CELTMode *m, celt_norm *X, int *average, int last_decision, int end, int _C, int M)
289 {
290    int i, c, N0;
291    int sum = 0, nbBands=0;
292    const int C = CHANNELS(_C);
293    const celt_int16 * restrict eBands = m->eBands;
294    int decision;
295    
296    N0 = M*m->shortMdctSize;
297
298    if (M*(eBands[end]-eBands[end-1]) <= 8)
299       return SPREAD_NONE;
300    c=0; do {
301       for (i=0;i<end;i++)
302       {
303          int j, N, tmp=0;
304          int tcount[3] = {0};
305          celt_norm * restrict x = X+M*eBands[i]+c*N0;
306          N = M*(eBands[i+1]-eBands[i]);
307          if (N<=8)
308             continue;
309          /* Compute rough CDF of |x[j]| */
310          for (j=0;j<N;j++)
311          {
312             celt_word32 x2N; /* Q13 */
313
314             x2N = MULT16_16(MULT16_16_Q15(x[j], x[j]), N);
315             if (x2N < QCONST16(0.25f,13))
316                tcount[0]++;
317             if (x2N < QCONST16(0.0625f,13))
318                tcount[1]++;
319             if (x2N < QCONST16(0.015625f,13))
320                tcount[2]++;
321          }
322
323          tmp = (2*tcount[2] >= N) + (2*tcount[1] >= N) + (2*tcount[0] >= N);
324          sum += tmp*256;
325          nbBands++;
326       }
327    } while (++c<C);
328    sum /= nbBands;
329    /* Recursive averaging */
330    sum = (sum+*average)>>1;
331    *average = sum;
332    /* Hysteresis */
333    sum = (3*sum + (((3-last_decision)<<7) + 64) + 2)>>2;
334    if (sum < 80)
335    {
336       decision = SPREAD_AGGRESSIVE;
337    } else if (sum < 256)
338    {
339       decision = SPREAD_NORMAL;
340    } else if (sum < 384)
341    {
342       decision = SPREAD_LIGHT;
343    } else {
344       decision = SPREAD_NONE;
345    }
346    return decision;
347 }
348
349 #ifdef MEASURE_NORM_MSE
350
351 float MSE[30] = {0};
352 int nbMSEBands = 0;
353 int MSECount[30] = {0};
354
355 void dump_norm_mse(void)
356 {
357    int i;
358    for (i=0;i<nbMSEBands;i++)
359    {
360       printf ("%g ", MSE[i]/MSECount[i]);
361    }
362    printf ("\n");
363 }
364
365 void measure_norm_mse(const CELTMode *m, float *X, float *X0, float *bandE, float *bandE0, int M, int N, int C)
366 {
367    static int init = 0;
368    int i;
369    if (!init)
370    {
371       atexit(dump_norm_mse);
372       init = 1;
373    }
374    for (i=0;i<m->nbEBands;i++)
375    {
376       int j;
377       int c;
378       float g;
379       if (bandE0[i]<10 || (C==2 && bandE0[i+m->nbEBands]<1))
380          continue;
381       c=0; do {
382          g = bandE[i+c*m->nbEBands]/(1e-15+bandE0[i+c*m->nbEBands]);
383          for (j=M*m->eBands[i];j<M*m->eBands[i+1];j++)
384             MSE[i] += (g*X[j+c*N]-X0[j+c*N])*(g*X[j+c*N]-X0[j+c*N]);
385       } while (++c<C);
386       MSECount[i]+=C;
387    }
388    nbMSEBands = m->nbEBands;
389 }
390
391 #endif
392
393 /* Indexing table for converting from natural Hadamard to ordery Hadamard
394    This is essentially a bit-reversed Gray, on top of which we've added
395    an inversion of the order because we want the DC at the end rather than
396    the beginning. The lines are for N=2, 4, 8, 16 */
397 static const int ordery_table[] = {
398        1,  0,
399        3,  0,  2,  1,
400        7,  0,  4,  3,  6,  1,  5,  2,
401       15,  0,  8,  7, 12,  3, 11,  4, 14,  1,  9,  6, 13,  2, 10,  5,
402 };
403
404 static void deinterleave_hadamard(celt_norm *X, int N0, int stride, int hadamard)
405 {
406    int i,j;
407    VARDECL(celt_norm, tmp);
408    int N;
409    SAVE_STACK;
410    N = N0*stride;
411    ALLOC(tmp, N, celt_norm);
412    if (hadamard)
413    {
414       const int *ordery = ordery_table+stride-2;
415       for (i=0;i<stride;i++)
416       {
417          for (j=0;j<N0;j++)
418             tmp[ordery[i]*N0+j] = X[j*stride+i];
419       }
420    } else {
421       for (i=0;i<stride;i++)
422          for (j=0;j<N0;j++)
423             tmp[i*N0+j] = X[j*stride+i];
424    }
425    for (j=0;j<N;j++)
426       X[j] = tmp[j];
427    RESTORE_STACK;
428 }
429
430 static void interleave_hadamard(celt_norm *X, int N0, int stride, int hadamard)
431 {
432    int i,j;
433    VARDECL(celt_norm, tmp);
434    int N;
435    SAVE_STACK;
436    N = N0*stride;
437    ALLOC(tmp, N, celt_norm);
438    if (hadamard)
439    {
440       const int *ordery = ordery_table+stride-2;
441       for (i=0;i<stride;i++)
442          for (j=0;j<N0;j++)
443             tmp[j*stride+i] = X[ordery[i]*N0+j];
444    } else {
445       for (i=0;i<stride;i++)
446          for (j=0;j<N0;j++)
447             tmp[j*stride+i] = X[i*N0+j];
448    }
449    for (j=0;j<N;j++)
450       X[j] = tmp[j];
451    RESTORE_STACK;
452 }
453
454 void haar1(celt_norm *X, int N0, int stride)
455 {
456    int i, j;
457    N0 >>= 1;
458    for (i=0;i<stride;i++)
459       for (j=0;j<N0;j++)
460       {
461          celt_norm tmp1, tmp2;
462          tmp1 = MULT16_16_Q15(QCONST16(.7070678f,15), X[stride*2*j+i]);
463          tmp2 = MULT16_16_Q15(QCONST16(.7070678f,15), X[stride*(2*j+1)+i]);
464          X[stride*2*j+i] = tmp1 + tmp2;
465          X[stride*(2*j+1)+i] = tmp1 - tmp2;
466       }
467 }
468
469 static int compute_qn(int N, int b, int offset, int stereo)
470 {
471    static const celt_int16 exp2_table8[8] =
472       {16384, 17866, 19483, 21247, 23170, 25267, 27554, 30048};
473    int qn, qb;
474    int N2 = 2*N-1;
475    if (stereo && N==2)
476       N2--;
477    qb = (b+N2*offset)/N2;
478    if (qb > (b>>1)-(1<<BITRES))
479       qb = (b>>1)-(1<<BITRES);
480
481    if (qb<0)
482        qb = 0;
483    if (qb>8<<BITRES)
484      qb = 8<<BITRES;
485
486    if (qb<(1<<BITRES>>1)) {
487       qn = 1;
488    } else {
489       qn = exp2_table8[qb&0x7]>>(14-(qb>>BITRES));
490       qn = (qn+1)>>1<<1;
491    }
492    celt_assert(qn <= 256);
493    return qn;
494 }
495
496 static celt_uint32 lcg_rand(celt_uint32 seed)
497 {
498    return 1664525 * seed + 1013904223;
499 }
500
501 /* This function is responsible for encoding and decoding a band for both
502    the mono and stereo case. Even in the mono case, it can split the band
503    in two and transmit the energy difference with the two half-bands. It
504    can be called recursively so bands can end up being split in 8 parts. */
505 static void quant_band(int encode, const CELTMode *m, int i, celt_norm *X, celt_norm *Y,
506       int N, int b, int spread, int B, int intensity, int tf_change, celt_norm *lowband, int resynth, void *ec,
507       celt_int32 *remaining_bits, int LM, celt_norm *lowband_out, const celt_ener *bandE, int level,
508       celt_int32 *seed, celt_word16 gain, celt_norm *lowband_scratch, int fill)
509 {
510    int q;
511    int curr_bits;
512    int stereo, split;
513    int imid=0, iside=0;
514    int N0=N;
515    int N_B=N;
516    int N_B0;
517    int B0=B;
518    int time_divide=0;
519    int recombine=0;
520    int inv = 0;
521    celt_word16 mid=0, side=0;
522    int longBlocks;
523
524    longBlocks = B0==1;
525
526    N_B /= B;
527    N_B0 = N_B;
528
529    split = stereo = Y != NULL;
530
531    /* Special case for one sample */
532    if (N==1)
533    {
534       int c;
535       celt_norm *x = X;
536       c=0; do {
537          int sign=0;
538          if (*remaining_bits>=1<<BITRES)
539          {
540             if (encode)
541             {
542                sign = x[0]<0;
543                ec_enc_bits((ec_enc*)ec, sign, 1);
544             } else {
545                sign = ec_dec_bits((ec_dec*)ec, 1);
546             }
547             *remaining_bits -= 1<<BITRES;
548             b-=1<<BITRES;
549          }
550          if (resynth)
551             x[0] = sign ? -NORM_SCALING : NORM_SCALING;
552          x = Y;
553       } while (++c<1+stereo);
554       if (lowband_out)
555          lowband_out[0] = SHR16(X[0],4);
556       return;
557    }
558
559    if (!stereo && level == 0)
560    {
561       int k;
562       if (tf_change>0)
563          recombine = tf_change;
564       /* Band recombining to increase frequency resolution */
565
566       if (lowband && (recombine || ((N_B&1) == 0 && tf_change<0) || B0>1))
567       {
568          int j;
569          for (j=0;j<N;j++)
570             lowband_scratch[j] = lowband[j];
571          lowband = lowband_scratch;
572       }
573
574       for (k=0;k<recombine;k++)
575       {
576          B>>=1;
577          N_B<<=1;
578          if (encode)
579             haar1(X, N_B, B);
580          if (lowband)
581             haar1(lowband, N_B, B);
582       }
583
584       /* Increasing the time resolution */
585       while ((N_B&1) == 0 && tf_change<0)
586       {
587          if (encode)
588             haar1(X, N_B, B);
589          if (lowband)
590             haar1(lowband, N_B, B);
591          B <<= 1;
592          N_B >>= 1;
593          time_divide++;
594          tf_change++;
595       }
596       B0=B;
597       N_B0 = N_B;
598
599       /* Reorganize the samples in time order instead of frequency order */
600       if (B0>1)
601       {
602          if (encode)
603             deinterleave_hadamard(X, N_B, B0, longBlocks);
604          if (lowband)
605             deinterleave_hadamard(lowband, N_B, B0, longBlocks);
606       }
607    }
608
609    /* If we need more than 32 bits, try splitting the band in two. */
610    if (!stereo && LM != -1 && b > 32<<BITRES && N>2)
611    {
612       if (LM>0 || (N&1)==0)
613       {
614          N >>= 1;
615          Y = X+N;
616          split = 1;
617          LM -= 1;
618          B = (B+1)>>1;
619       }
620    }
621
622    if (split)
623    {
624       int qn;
625       int itheta=0;
626       int mbits, sbits, delta;
627       int qalloc;
628       int offset;
629
630       /* Decide on the resolution to give to the split parameter theta */
631       offset = ((m->logN[i]+(LM<<BITRES))>>1) - (stereo ? QTHETA_OFFSET_STEREO : QTHETA_OFFSET);
632       qn = compute_qn(N, b, offset, stereo);
633       qalloc = 0;
634       if (stereo && i>=intensity)
635          qn = 1;
636       if (encode)
637       {
638          /* theta is the atan() of the ratio between the (normalized)
639             side and mid. With just that parameter, we can re-scale both
640             mid and side because we know that 1) they have unit norm and
641             2) they are orthogonal. */
642          itheta = stereo_itheta(X, Y, stereo, N);
643       }
644       if (qn!=1)
645       {
646          if (encode)
647             itheta = (itheta*qn+8192)>>14;
648
649          /* Entropy coding of the angle. We use a uniform pdf for the
650             first stereo split but a triangular one for the rest. */
651          if (stereo || B>1)
652          {
653             if (encode)
654                ec_enc_uint((ec_enc*)ec, itheta, qn+1);
655             else
656                itheta = ec_dec_uint((ec_dec*)ec, qn+1);
657             qalloc = log2_frac(qn+1,BITRES);
658          } else {
659             int fs=1, ft;
660             ft = ((qn>>1)+1)*((qn>>1)+1);
661             if (encode)
662             {
663                int fl;
664
665                fs = itheta <= (qn>>1) ? itheta + 1 : qn + 1 - itheta;
666                fl = itheta <= (qn>>1) ? itheta*(itheta + 1)>>1 :
667                 ft - ((qn + 1 - itheta)*(qn + 2 - itheta)>>1);
668
669                ec_encode((ec_enc*)ec, fl, fl+fs, ft);
670             } else {
671                int fl=0;
672                int fm;
673                fm = ec_decode((ec_dec*)ec, ft);
674
675                if (fm < ((qn>>1)*((qn>>1) + 1)>>1))
676                {
677                   itheta = (isqrt32(8*(celt_uint32)fm + 1) - 1)>>1;
678                   fs = itheta + 1;
679                   fl = itheta*(itheta + 1)>>1;
680                }
681                else
682                {
683                   itheta = (2*(qn + 1)
684                    - isqrt32(8*(celt_uint32)(ft - fm - 1) + 1))>>1;
685                   fs = qn + 1 - itheta;
686                   fl = ft - ((qn + 1 - itheta)*(qn + 2 - itheta)>>1);
687                }
688
689                ec_dec_update((ec_dec*)ec, fl, fl+fs, ft);
690             }
691             qalloc = log2_frac(ft,BITRES) - log2_frac(fs,BITRES) + 1;
692          }
693          itheta = (celt_int32)itheta*16384/qn;
694          if (encode && stereo)
695             stereo_split(X, Y, N);
696          /* TODO: Renormalising X and Y *may* help fixed-point a bit at very high rate.
697                   Let's do that at higher complexity */
698       } else if (stereo) {
699          if (encode)
700          {
701             inv = itheta > 8192;
702             if (inv)
703             {
704                int j;
705                for (j=0;j<N;j++)
706                   Y[j] = -Y[j];
707             }
708             intensity_stereo(m, X, Y, bandE, i, N);
709          }
710          if (b>2<<BITRES && *remaining_bits > 2<<BITRES)
711          {
712             if (encode)
713                ec_enc_bit_logp(ec, inv, 2);
714             else
715                inv = ec_dec_bit_logp(ec, 2);
716             qalloc = inv ? 16 : 4;
717          } else
718             inv = 0;
719          itheta = 0;
720       }
721
722       if (itheta == 0)
723       {
724          imid = 32767;
725          iside = 0;
726          delta = -10000;
727       } else if (itheta == 16384)
728       {
729          imid = 0;
730          iside = 32767;
731          delta = 10000;
732       } else {
733          imid = bitexact_cos(itheta);
734          iside = bitexact_cos(16384-itheta);
735          /* This is the mid vs side allocation that minimizes squared error
736             in that band. */
737          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
738       }
739
740 #ifdef FIXED_POINT
741       mid = imid;
742       side = iside;
743 #else
744       mid = (1.f/32768)*imid;
745       side = (1.f/32768)*iside;
746 #endif
747
748       /* This is a special case for N=2 that only works for stereo and takes
749          advantage of the fact that mid and side are orthogonal to encode
750          the side with just one bit. */
751       if (N==2 && stereo)
752       {
753          int c;
754          int sign=1;
755          celt_norm *x2, *y2;
756          mbits = b-qalloc;
757          sbits = 0;
758          /* Only need one bit for the side */
759          if (itheta != 0 && itheta != 16384)
760             sbits = 1<<BITRES;
761          mbits -= sbits;
762          c = itheta > 8192;
763          *remaining_bits -= qalloc+sbits;
764
765          x2 = c ? Y : X;
766          y2 = c ? X : Y;
767          if (sbits)
768          {
769             if (encode)
770             {
771                /* Here we only need to encode a sign for the side */
772                sign = x2[0]*y2[1] - x2[1]*y2[0] > 0;
773                ec_enc_bits((ec_enc*)ec, sign, 1);
774             } else {
775                sign = ec_dec_bits((ec_dec*)ec, 1);
776             }
777          }
778          sign = 2*sign - 1;
779          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);
780          y2[0] = -sign*x2[1];
781          y2[1] = sign*x2[0];
782          if (resynth)
783          {
784             celt_norm tmp;
785             X[0] = MULT16_16_Q15(mid, X[0]);
786             X[1] = MULT16_16_Q15(mid, X[1]);
787             Y[0] = MULT16_16_Q15(side, Y[0]);
788             Y[1] = MULT16_16_Q15(side, Y[1]);
789             tmp = X[0];
790             X[0] = SUB16(tmp,Y[0]);
791             Y[0] = ADD16(tmp,Y[0]);
792             tmp = X[1];
793             X[1] = SUB16(tmp,Y[1]);
794             Y[1] = ADD16(tmp,Y[1]);
795          }
796       } else {
797          /* "Normal" split code */
798          celt_norm *next_lowband2=NULL;
799          celt_norm *next_lowband_out1=NULL;
800          int next_level=0;
801
802          /* Give more bits to low-energy MDCTs than they would otherwise deserve */
803          if (B>1 && !stereo)
804          {
805             if (itheta > 8192)
806                delta -= delta>>(4+level-LM);
807             else
808                delta -= delta>>(5+level-LM);
809          }
810          mbits = (b-qalloc-delta)/2;
811          if (mbits > b-qalloc)
812             mbits = b-qalloc;
813          if (mbits<0)
814             mbits=0;
815          sbits = b-qalloc-mbits;
816          *remaining_bits -= qalloc;
817
818          if (lowband && !stereo)
819             next_lowband2 = lowband+N; /* >32-bit split case */
820
821          /* Only stereo needs to pass on lowband_out. Otherwise, it's
822             handled at the end */
823          if (stereo)
824             next_lowband_out1 = lowband_out;
825          else
826             next_level = level+1;
827
828          quant_band(encode, m, i, X, NULL, N, mbits, spread, B, intensity, tf_change,
829                lowband, resynth, ec, remaining_bits, LM, next_lowband_out1,
830                NULL, next_level, seed, MULT16_16_P15(gain,mid), lowband_scratch, fill);
831          quant_band(encode, m, i, Y, NULL, N, sbits, spread, B, intensity, tf_change,
832                next_lowband2, resynth, ec, remaining_bits, LM, NULL,
833                NULL, next_level, seed, MULT16_16_P15(gain,side), NULL, fill && !stereo);
834       }
835
836    } else {
837       /* This is the basic no-split case */
838       q = bits2pulses(m, i, LM, b);
839       curr_bits = pulses2bits(m, i, LM, q);
840       *remaining_bits -= curr_bits;
841
842       /* Ensures we can never bust the budget */
843       while (*remaining_bits < 0 && q > 0)
844       {
845          *remaining_bits += curr_bits;
846          q--;
847          curr_bits = pulses2bits(m, i, LM, q);
848          *remaining_bits -= curr_bits;
849       }
850
851       if (q!=0)
852       {
853          int K = get_pulses(q);
854
855          /* Finally do the actual quantization */
856          if (encode)
857             alg_quant(X, N, K, spread, B, lowband, resynth, (ec_enc*)ec, seed, gain);
858          else
859             alg_unquant(X, N, K, spread, B, lowband, (ec_dec*)ec, seed, gain);
860       } else {
861          /* If there's no pulse, fill the band anyway */
862          int j;
863          if (resynth)
864          {
865             if (!fill)
866             {
867                for (j=0;j<N;j++)
868                   X[j] = 0;
869             } else {
870                if (lowband == NULL || (spread==SPREAD_AGGRESSIVE && B<=1))
871                {
872                   /* Noise */
873                   for (j=0;j<N;j++)
874                   {
875                      *seed = lcg_rand(*seed);
876                      X[j] = (int)(*seed)>>20;
877                   }
878                } else {
879                   /* Folded spectrum */
880                   for (j=0;j<N;j++)
881                      X[j] = lowband[j];
882                }
883                renormalise_vector(X, N, gain);
884             }
885          }
886       }
887    }
888
889    /* This code is used by the decoder and by the resynthesis-enabled encoder */
890    if (resynth)
891    {
892       if (stereo)
893       {
894          if (N!=2)
895             stereo_merge(X, Y, mid, N);
896          if (inv)
897          {
898             int j;
899             for (j=0;j<N;j++)
900                Y[j] = -Y[j];
901          }
902       } else if (level == 0)
903       {
904          int k;
905
906          /* Undo the sample reorganization going from time order to frequency order */
907          if (B0>1)
908             interleave_hadamard(X, N_B, B0, longBlocks);
909
910          /* Undo time-freq changes that we did earlier */
911          N_B = N_B0;
912          B = B0;
913          for (k=0;k<time_divide;k++)
914          {
915             B >>= 1;
916             N_B <<= 1;
917             haar1(X, N_B, B);
918          }
919
920          for (k=0;k<recombine;k++)
921          {
922             haar1(X, N_B, B);
923             N_B>>=1;
924             B <<= 1;
925          }
926
927          /* Scale output for later folding */
928          if (lowband_out)
929          {
930             int j;
931             celt_word16 n;
932             n = celt_sqrt(SHL32(EXTEND32(N0),22));
933             for (j=0;j<N0;j++)
934                lowband_out[j] = MULT16_16_Q15(n,X[j]);
935          }
936       }
937    }
938 }
939
940 void quant_all_bands(int encode, const CELTMode *m, int start, int end,
941       celt_norm *_X, celt_norm *_Y, const celt_ener *bandE, int *pulses,
942       int shortBlocks, int spread, int dual_stereo, int intensity, int *tf_res, int resynth,
943       int total_bits, void *ec, int LM, int codedBands)
944 {
945    int i, balance;
946    celt_int32 remaining_bits;
947    const celt_int16 * restrict eBands = m->eBands;
948    celt_norm * restrict norm, * restrict norm2;
949    VARDECL(celt_norm, _norm);
950    VARDECL(celt_norm, lowband_scratch);
951    int B;
952    int M;
953    celt_int32 seed;
954    int lowband_offset;
955    int update_lowband = 1;
956    int C = _Y != NULL ? 2 : 1;
957    SAVE_STACK;
958
959    M = 1<<LM;
960    B = shortBlocks ? M : 1;
961    ALLOC(_norm, C*M*eBands[m->nbEBands], celt_norm);
962    ALLOC(lowband_scratch, M*(eBands[m->nbEBands]-eBands[m->nbEBands-1]), celt_norm);
963    norm = _norm;
964    norm2 = norm + M*eBands[m->nbEBands];
965 #if 0
966    if (C==2)
967    {
968       int j;
969       int left = 0;
970       for (j=intensity;j<codedBands;j++)
971       {
972          int tmp = pulses[j]/2;
973          left += tmp;
974          pulses[j] -= tmp;
975       }
976       if (codedBands) {
977          int perband;
978          perband = left/(m->eBands[codedBands]-m->eBands[start]);
979          for (j=start;j<codedBands;j++)
980             pulses[j] += perband*(m->eBands[j+1]-m->eBands[j]);
981          left = left-(m->eBands[codedBands]-m->eBands[start])*perband;
982          for (j=start;j<codedBands;j++)
983          {
984             int tmp = IMIN(left, m->eBands[j+1]-m->eBands[j]);
985             pulses[j] += tmp;
986             left -= tmp;
987          }
988       }
989    }
990 #endif
991    if (encode)
992       seed = ((ec_enc*)ec)->rng;
993    else
994       seed = ((ec_dec*)ec)->rng;
995    balance = 0;
996    lowband_offset = -1;
997    for (i=start;i<end;i++)
998    {
999       int tell;
1000       int b;
1001       int N;
1002       int curr_balance;
1003       int effective_lowband=-1;
1004       celt_norm * restrict X, * restrict Y;
1005       int tf_change=0;
1006       
1007       X = _X+M*eBands[i];
1008       if (_Y!=NULL)
1009          Y = _Y+M*eBands[i];
1010       else
1011          Y = NULL;
1012       N = M*eBands[i+1]-M*eBands[i];
1013       if (encode)
1014          tell = ec_enc_tell((ec_enc*)ec, BITRES);
1015       else
1016          tell = ec_dec_tell((ec_dec*)ec, BITRES);
1017
1018       /* Compute how many bits we want to allocate to this band */
1019       if (i != start)
1020          balance -= tell;
1021       remaining_bits = (total_bits<<BITRES)-tell-1;
1022       if (i <= codedBands-1)
1023       {
1024          curr_balance = (codedBands-i);
1025          if (curr_balance > 3)
1026             curr_balance = 3;
1027          curr_balance = balance / curr_balance;
1028          b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
1029          if (b<0)
1030             b = 0;
1031       } else {
1032          b = 0;
1033       }
1034
1035       if (M*eBands[i]-N >= M*eBands[start] && (update_lowband || lowband_offset==-1))
1036             lowband_offset = M*eBands[i];
1037
1038       tf_change = tf_res[i];
1039       if (i>=m->effEBands)
1040       {
1041          X=norm;
1042          if (_Y!=NULL)
1043             Y = norm;
1044       }
1045
1046       /* This ensures we never repeat spectral content within one band */
1047       if (lowband_offset != -1)
1048       {
1049          effective_lowband = lowband_offset-N;
1050          if (effective_lowband < M*eBands[start])
1051             effective_lowband = M*eBands[start];
1052       }
1053       if (dual_stereo && i==intensity)
1054       {
1055          int j;
1056
1057          /* Switch off dual stereo to do intensity */
1058          dual_stereo = 0;
1059          for (j=M*eBands[start];j<M*eBands[i];j++)
1060             norm[j] = HALF32(norm[j]+norm2[j]);
1061       }
1062       if (dual_stereo)
1063       {
1064          quant_band(encode, m, i, X, NULL, N, b/2, spread, B, intensity, tf_change,
1065                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1066                norm+M*eBands[i], bandE, 0, &seed, Q15ONE, lowband_scratch, 1);
1067          quant_band(encode, m, i, Y, NULL, N, b/2, spread, B, intensity, tf_change,
1068                effective_lowband != -1 ? norm2+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1069                norm2+M*eBands[i], bandE, 0, &seed, Q15ONE, lowband_scratch, 1);
1070       } else {
1071          quant_band(encode, m, i, X, Y, N, b, spread, B, intensity, tf_change,
1072                effective_lowband != -1 ? norm+effective_lowband : NULL, resynth, ec, &remaining_bits, LM,
1073                norm+M*eBands[i], bandE, 0, &seed, Q15ONE, lowband_scratch, 1);
1074       }
1075       balance += pulses[i] + tell;
1076
1077       /* Update the folding position only as long as we have 1 bit/sample depth */
1078       update_lowband = (b>>BITRES)>N;
1079    }
1080    RESTORE_STACK;
1081 }
1082