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