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