This should prevent a rare divide-by-zero in the pitch gain code
[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
49 #ifdef FIXED_POINT
50 /* Compute the amplitude (sqrt energy) in each of the bands */
51 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int _C)
52 {
53    int i, c, N;
54    const celt_int16 *eBands = m->eBands;
55    const int C = CHANNELS(_C);
56    N = FRAMESIZE(m);
57    for (c=0;c<C;c++)
58    {
59       for (i=0;i<m->nbEBands;i++)
60       {
61          int j;
62          celt_word32 maxval=0;
63          celt_word32 sum = 0;
64          
65          j=eBands[i]; do {
66             maxval = MAX32(maxval, X[j+c*N]);
67             maxval = MAX32(maxval, -X[j+c*N]);
68          } while (++j<eBands[i+1]);
69          
70          if (maxval > 0)
71          {
72             int shift = celt_ilog2(maxval)-10;
73             j=eBands[i]; do {
74                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
75                                    EXTRACT16(VSHR32(X[j+c*N],shift)));
76             } while (++j<eBands[i+1]);
77             /* We're adding one here to make damn sure we never end up with a pitch vector that's
78                larger than unity norm */
79             bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
80          } else {
81             bank[i+c*m->nbEBands] = EPSILON;
82          }
83          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
84       }
85    }
86    /*printf ("\n");*/
87 }
88
89 /* Normalise each band such that the energy is one. */
90 void normalise_bands(const CELTMode *m, const celt_sig * restrict freq, celt_norm * restrict X, const celt_ener *bank, int _C)
91 {
92    int i, c, N;
93    const celt_int16 *eBands = m->eBands;
94    const int C = CHANNELS(_C);
95    N = FRAMESIZE(m);
96    for (c=0;c<C;c++)
97    {
98       i=0; do {
99          celt_word16 g;
100          int j,shift;
101          celt_word16 E;
102          shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
103          E = VSHR32(bank[i+c*m->nbEBands], shift);
104          g = EXTRACT16(celt_rcp(SHL32(E,3)));
105          j=eBands[i]; do {
106             X[j+c*N] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
107          } while (++j<eBands[i+1]);
108       } while (++i<m->nbEBands);
109    }
110 }
111
112 #else /* FIXED_POINT */
113 /* Compute the amplitude (sqrt energy) in each of the bands */
114 void compute_band_energies(const CELTMode *m, const celt_sig *X, celt_ener *bank, int _C)
115 {
116    int i, c, N;
117    const celt_int16 *eBands = m->eBands;
118    const int C = CHANNELS(_C);
119    N = FRAMESIZE(m);
120    for (c=0;c<C;c++)
121    {
122       for (i=0;i<m->nbEBands;i++)
123       {
124          int j;
125          celt_word32 sum = 1e-10;
126          for (j=eBands[i];j<eBands[i+1];j++)
127             sum += X[j+c*N]*X[j+c*N];
128          bank[i+c*m->nbEBands] = sqrt(sum);
129          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
130       }
131    }
132    /*printf ("\n");*/
133 }
134
135 #ifdef EXP_PSY
136 void compute_noise_energies(const CELTMode *m, const celt_sig *X, const celt_word16 *tonality, celt_ener *bank, int _C)
137 {
138    int i, c, N;
139    const celt_int16 *eBands = m->eBands;
140    const int C = CHANNELS(_C);
141    N = FRAMESIZE(m);
142    for (c=0;c<C;c++)
143    {
144       for (i=0;i<m->nbEBands;i++)
145       {
146          int j;
147          celt_word32 sum = 1e-10;
148          for (j=eBands[i];j<eBands[i+1];j++)
149             sum += X[j*C+c]*X[j+c*N]*tonality[j];
150          bank[i+c*m->nbEBands] = sqrt(sum);
151          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
152       }
153    }
154    /*printf ("\n");*/
155 }
156 #endif
157
158 /* Normalise each band such that the energy is one. */
159 void normalise_bands(const CELTMode *m, const celt_sig * restrict freq, celt_norm * restrict X, const celt_ener *bank, int _C)
160 {
161    int i, c, N;
162    const celt_int16 *eBands = m->eBands;
163    const int C = CHANNELS(_C);
164    N = FRAMESIZE(m);
165    for (c=0;c<C;c++)
166    {
167       for (i=0;i<m->nbEBands;i++)
168       {
169          int j;
170          celt_word16 g = 1.f/(1e-10+bank[i+c*m->nbEBands]);
171          for (j=eBands[i];j<eBands[i+1];j++)
172             X[j+c*N] = freq[j+c*N]*g;
173       }
174    }
175 }
176
177 #endif /* FIXED_POINT */
178
179 void renormalise_bands(const CELTMode *m, celt_norm * restrict X, int _C)
180 {
181    int i, c;
182    const celt_int16 *eBands = m->eBands;
183    const int C = CHANNELS(_C);
184    for (c=0;c<C;c++)
185    {
186       i=0; do {
187          renormalise_vector(X+eBands[i]+c*m->eBands[m->nbEBands+1], Q15ONE, eBands[i+1]-eBands[i], 1);
188       } while (++i<m->nbEBands);
189    }
190 }
191
192 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
193 void denormalise_bands(const CELTMode *m, const celt_norm * restrict X, celt_sig * restrict freq, const celt_ener *bank, int _C)
194 {
195    int i, c, N;
196    const celt_int16 *eBands = m->eBands;
197    const int C = CHANNELS(_C);
198    N = FRAMESIZE(m);
199    if (C>2)
200       celt_fatal("denormalise_bands() not implemented for >2 channels");
201    for (c=0;c<C;c++)
202    {
203       for (i=0;i<m->nbEBands;i++)
204       {
205          int j;
206          celt_word32 g = SHR32(bank[i+c*m->nbEBands],1);
207          j=eBands[i]; do {
208             freq[j+c*N] = SHL32(MULT16_32_Q15(X[j+c*N], g),2);
209          } while (++j<eBands[i+1]);
210       }
211       for (i=eBands[m->nbEBands];i<eBands[m->nbEBands+1];i++)
212          freq[i+c*N] = 0;
213    }
214 }
215
216 int compute_pitch_gain(const CELTMode *m, const celt_sig *X, const celt_sig *P, int norm_rate, int *gain_id, int _C, celt_word16 *gain_prod)
217 {
218    int j, c;
219    celt_word16 g;
220    celt_word16 delta;
221    const int C = CHANNELS(_C);
222    celt_word32 Sxy=0, Sxx=0, Syy=0;
223    int len = m->pitchEnd;
224    const int N = FRAMESIZE(m);
225 #ifdef FIXED_POINT
226    int shift = 0;
227    celt_word32 maxabs=0;
228
229    for (c=0;c<C;c++)
230    {
231       for (j=0;j<len;j++)
232       {
233          maxabs = MAX32(maxabs, ABS32(X[j+c*N]));
234          maxabs = MAX32(maxabs, ABS32(P[j+c*N]));
235       }
236    }
237    shift = celt_ilog2(maxabs)-12;
238    if (shift<0)
239       shift = 0;
240 #endif
241    delta = PDIV32_16(Q15ONE, len);
242    for (c=0;c<C;c++)
243    {
244       celt_word16 gg = Q15ONE;
245       for (j=0;j<len;j++)
246       {
247          celt_word16 Xj, Pj;
248          Xj = EXTRACT16(SHR32(X[j+c*N], shift));
249          Pj = MULT16_16_P15(gg,EXTRACT16(SHR32(P[j+c*N], shift)));
250          Sxy = MAC16_16(Sxy, Xj, Pj);
251          Sxx = MAC16_16(Sxx, Pj, Pj);
252          Syy = MAC16_16(Syy, Xj, Xj);
253          gg = SUB16(gg, delta);
254       }
255    }
256 #ifdef FIXED_POINT
257    {
258       celt_word32 num, den;
259       celt_word16 fact;
260       fact = MULT16_16(QCONST16(.04, 14), norm_rate);
261       if (fact < QCONST16(1., 14))
262          fact = QCONST16(1., 14);
263       num = Sxy;
264       den = EPSILON+Sxx+MULT16_32_Q15(QCONST16(.03,15),Syy);
265       shift = celt_zlog2(Sxy)-16;
266       if (shift < 0)
267          shift = 0;
268       if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
269          g = 0;
270       else
271          g = DIV32(SHL32(SHR32(num,shift),14),ADD32(EPSILON,SHR32(den,shift)));
272
273       /* This MUST round down so that we don't over-estimate the gain */
274       *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
275    }
276 #else
277    {
278       float fact = .04*norm_rate;
279       if (fact < 1)
280          fact = 1;
281       g = Sxy/(.1+Sxx+.03*Syy);
282       if (Sxy < .5*fact*celt_sqrt(1+Sxx*Syy))
283          g = 0;
284       /* This MUST round down so that we don't over-estimate the gain */
285       *gain_id = floor(20*(g-.5));
286    }
287 #endif
288    /* This prevents the pitch gain from being above 1.0 for too long by bounding the 
289       maximum error amplification factor to 2.0 */
290    g = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),*gain_id));
291    *gain_prod = MAX16(QCONST32(1., 13), MULT16_16_Q14(*gain_prod,g));
292    if (*gain_prod>QCONST32(2., 13))
293    {
294       *gain_id=9;
295       *gain_prod = QCONST32(2., 13);
296    }
297
298    if (*gain_id < 0)
299    {
300       *gain_id = 0;
301       return 0;
302    } else {
303       if (*gain_id > 15)
304          *gain_id = 15;
305       return 1;
306    }
307 }
308
309 void apply_pitch(const CELTMode *m, celt_sig *X, const celt_sig *P, int gain_id, int pred, int _C)
310 {
311    int j, c, N;
312    celt_word16 gain;
313    celt_word16 delta;
314    const int C = CHANNELS(_C);
315    int len = m->pitchEnd;
316
317    N = FRAMESIZE(m);
318    gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
319    delta = PDIV32_16(gain, len);
320    if (pred)
321       gain = -gain;
322    else
323       delta = -delta;
324    for (c=0;c<C;c++)
325    {
326       celt_word16 gg = gain;
327       for (j=0;j<len;j++)
328       {
329          X[j+c*N] += SHL32(MULT16_32_Q15(gg,P[j+c*N]),1);
330          gg = ADD16(gg, delta);
331       }
332    }
333 }
334
335 #ifndef DISABLE_STEREO
336
337 static void stereo_band_mix(const CELTMode *m, celt_norm *X, celt_norm *Y, const celt_ener *bank, int stereo_mode, int bandID, int dir)
338 {
339    int i = bandID;
340    const celt_int16 *eBands = m->eBands;
341    int j;
342    celt_word16 a1, a2;
343    if (stereo_mode==0)
344    {
345       /* Do mid-side when not doing intensity stereo */
346       a1 = QCONST16(.70711f,14);
347       a2 = dir*QCONST16(.70711f,14);
348    } else {
349       celt_word16 left, right;
350       celt_word16 norm;
351 #ifdef FIXED_POINT
352       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
353 #endif
354       left = VSHR32(bank[i],shift);
355       right = VSHR32(bank[i+m->nbEBands],shift);
356       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
357       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
358       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
359    }
360    for (j=0;j<eBands[i+1]-eBands[i];j++)
361    {
362       celt_norm r, l;
363       l = X[j];
364       r = Y[j];
365       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
366       Y[j] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
367    }
368 }
369
370
371 #endif /* DISABLE_STEREO */
372
373 int folding_decision(const CELTMode *m, celt_norm *X, celt_word16 *average, int *last_decision, int _C)
374 {
375    int i, c, N0;
376    int NR=0;
377    celt_word32 ratio = EPSILON;
378    const int C = CHANNELS(_C);
379    const celt_int16 * restrict eBands = m->eBands;
380    
381    N0 = FRAMESIZE(m);
382
383    for (c=0;c<C;c++)
384    {
385    for (i=0;i<m->nbEBands;i++)
386    {
387       int j, N;
388       int max_i=0;
389       celt_word16 max_val=EPSILON;
390       celt_word32 floor_ener=EPSILON;
391       celt_norm * restrict x = X+eBands[i]+c*N0;
392       N = eBands[i+1]-eBands[i];
393       for (j=0;j<N;j++)
394       {
395          if (ABS16(x[j])>max_val)
396          {
397             max_val = ABS16(x[j]);
398             max_i = j;
399          }
400       }
401 #if 0
402       for (j=0;j<N;j++)
403       {
404          if (abs(j-max_i)>2)
405             floor_ener += x[j]*x[j];
406       }
407 #else
408       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
409       if (max_i < N-1)
410          floor_ener -= MULT16_16(x[(max_i+1)], x[(max_i+1)]);
411       if (max_i < N-2)
412          floor_ener -= MULT16_16(x[(max_i+2)], x[(max_i+2)]);
413       if (max_i > 0)
414          floor_ener -= MULT16_16(x[(max_i-1)], x[(max_i-1)]);
415       if (max_i > 1)
416          floor_ener -= MULT16_16(x[(max_i-2)], x[(max_i-2)]);
417       floor_ener = MAX32(floor_ener, EPSILON);
418 #endif
419       if (N>7)
420       {
421          celt_word16 r;
422          celt_word16 den = celt_sqrt(floor_ener);
423          den = MAX32(QCONST16(.02, 15), den);
424          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
425          ratio = ADD32(ratio, EXTEND32(r));
426          NR++;
427       }
428    }
429    }
430    if (NR>0)
431       ratio = DIV32_16(ratio, NR);
432    ratio = ADD32(HALF32(ratio), HALF32(*average));
433    if (!*last_decision)
434    {
435       *last_decision = (ratio < QCONST16(1.8,8));
436    } else {
437       *last_decision = (ratio < QCONST16(3.,8));
438    }
439    *average = EXTRACT16(ratio);
440    return *last_decision;
441 }
442
443 /* Quantisation of the residual */
444 void quant_bands(const CELTMode *m, celt_norm * restrict X, const celt_ener *bandE, int *pulses, int shortBlocks, int fold, int total_bits, int encode, void *enc_dec)
445 {
446    int i, j, remaining_bits, balance;
447    const celt_int16 * restrict eBands = m->eBands;
448    celt_norm * restrict norm;
449    VARDECL(celt_norm, _norm);
450    int B;
451    SAVE_STACK;
452
453    B = shortBlocks ? m->nbShortMdcts : 1;
454    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm);
455    norm = _norm;
456
457    balance = 0;
458    for (i=0;i<m->nbEBands;i++)
459    {
460       int tell;
461       int N;
462       int q;
463       celt_word16 n;
464       const celt_int16 * const *BPbits;
465       
466       int curr_balance, curr_bits;
467       
468       N = eBands[i+1]-eBands[i];
469       BPbits = m->bits;
470
471       if (encode)
472          tell = ec_enc_tell(enc_dec, BITRES);
473       else
474          tell = ec_dec_tell(enc_dec, BITRES);
475       if (i != 0)
476          balance -= tell;
477       remaining_bits = (total_bits<<BITRES)-tell-1;
478       curr_balance = (m->nbEBands-i);
479       if (curr_balance > 3)
480          curr_balance = 3;
481       curr_balance = balance / curr_balance;
482       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
483       curr_bits = pulses2bits(BPbits[i], N, q);
484       remaining_bits -= curr_bits;
485       while (remaining_bits < 0 && q > 0)
486       {
487          remaining_bits += curr_bits;
488          q--;
489          curr_bits = pulses2bits(BPbits[i], N, q);
490          remaining_bits -= curr_bits;
491       }
492       balance += pulses[i] + tell;
493       
494       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
495
496       if (q > 0)
497       {
498          int spread = fold ? B : 0;
499          if (encode)
500             alg_quant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc_dec);
501          else
502             alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc_dec);
503       } else {
504          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
505       }
506       for (j=eBands[i];j<eBands[i+1];j++)
507          norm[j] = MULT16_16_Q15(n,X[j]);
508    }
509    RESTORE_STACK;
510 }
511
512 #ifndef DISABLE_STEREO
513
514 void quant_bands_stereo(const CELTMode *m, celt_norm *_X, const celt_ener *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
515 {
516    int i, j, remaining_bits, balance;
517    const celt_int16 * restrict eBands = m->eBands;
518    celt_norm * restrict norm;
519    VARDECL(celt_norm, _norm);
520    int B;
521    celt_word16 mid, side;
522    SAVE_STACK;
523
524    B = shortBlocks ? m->nbShortMdcts : 1;
525    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm);
526    norm = _norm;
527
528    balance = 0;
529    for (i=0;i<m->nbEBands;i++)
530    {
531       int c;
532       int tell;
533       int q1, q2;
534       celt_word16 n;
535       const celt_int16 * const *BPbits;
536       int b, qb;
537       int N;
538       int curr_balance, curr_bits;
539       int imid, iside, itheta;
540       int mbits, sbits, delta;
541       int qalloc;
542       celt_norm * restrict X, * restrict Y;
543       
544       X = _X+eBands[i];
545       Y = X+eBands[m->nbEBands+1];
546       BPbits = m->bits;
547
548       N = eBands[i+1]-eBands[i];
549       tell = ec_enc_tell(enc, BITRES);
550       if (i != 0)
551          balance -= tell;
552       remaining_bits = (total_bits<<BITRES)-tell-1;
553       curr_balance = (m->nbEBands-i);
554       if (curr_balance > 3)
555          curr_balance = 3;
556       curr_balance = balance / curr_balance;
557       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
558       if (b<0)
559          b = 0;
560
561       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
562       if (qb > (b>>BITRES)-1)
563          qb = (b>>BITRES)-1;
564       if (qb<0)
565          qb = 0;
566       if (qb>14)
567          qb = 14;
568
569       stereo_band_mix(m, X, Y, bandE, qb==0, i, 1);
570
571       mid = renormalise_vector(X, Q15ONE, N, 1);
572       side = renormalise_vector(Y, Q15ONE, N, 1);
573 #ifdef FIXED_POINT
574       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
575 #else
576       itheta = floor(.5+16384*0.63662*atan2(side,mid));
577 #endif
578       qalloc = log2_frac((1<<qb)+1,BITRES);
579       if (qb==0)
580       {
581          itheta=0;
582       } else {
583          int shift;
584          shift = 14-qb;
585          itheta = (itheta+(1<<shift>>1))>>shift;
586          ec_enc_uint(enc, itheta, (1<<qb)+1);
587          itheta <<= shift;
588       }
589       if (itheta == 0)
590       {
591          imid = 32767;
592          iside = 0;
593          delta = -10000;
594       } else if (itheta == 16384)
595       {
596          imid = 0;
597          iside = 32767;
598          delta = 10000;
599       } else {
600          imid = bitexact_cos(itheta);
601          iside = bitexact_cos(16384-itheta);
602          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
603       }
604       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
605
606       if (N==2)
607       {
608          int c2;
609          int sign=1;
610          celt_norm v[2], w[2];
611          celt_norm *x2, *y2;
612          mbits = b-qalloc;
613          sbits = 0;
614          if (itheta != 0 && itheta != 16384)
615             sbits = 1<<BITRES;
616          mbits -= sbits;
617          c = itheta > 8192 ? 1 : 0;
618          c2 = 1-c;
619
620          x2 = X;
621          y2 = Y;
622          if (c==0)
623          {
624             v[0] = x2[0];
625             v[1] = x2[1];
626             w[0] = y2[0];
627             w[1] = y2[1];
628          } else {
629             v[0] = y2[0];
630             v[1] = y2[1];
631             w[0] = x2[0];
632             w[1] = x2[1];
633          }
634          q1 = bits2pulses(m, BPbits[i], N, mbits);
635          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
636          remaining_bits -= curr_bits;
637          while (remaining_bits < 0 && q1 > 0)
638          {
639             remaining_bits += curr_bits;
640             q1--;
641             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
642             remaining_bits -= curr_bits;
643          }
644
645          if (q1 > 0)
646          {
647             int spread = fold ? B : 0;
648             alg_quant(v, N, q1, spread, enc);
649          } else {
650             v[0] = QCONST16(1.f, 14);
651             v[1] = 0;
652          }
653          if (sbits)
654          {
655             if (v[0]*w[1] - v[1]*w[0] > 0)
656                sign = 1;
657             else
658                sign = -1;
659             ec_enc_bits(enc, sign==1, 1);
660          } else {
661             sign = 1;
662          }
663          w[0] = -sign*v[1];
664          w[1] = sign*v[0];
665          if (c==0)
666          {
667             x2[0] = v[0];
668             x2[1] = v[1];
669             y2[0] = w[0];
670             y2[1] = w[1];
671          } else {
672             x2[0] = w[0];
673             x2[1] = w[1];
674             y2[0] = v[0];
675             y2[1] = v[1];
676          }
677       } else {
678          
679          mbits = (b-qalloc/2-delta)/2;
680          if (mbits > b-qalloc)
681             mbits = b-qalloc;
682          if (mbits<0)
683             mbits=0;
684          sbits = b-qalloc-mbits;
685          q1 = bits2pulses(m, BPbits[i], N, mbits);
686          q2 = bits2pulses(m, BPbits[i], N, sbits);
687          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
688          remaining_bits -= curr_bits;
689          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
690          {
691             remaining_bits += curr_bits;
692             if (q1>q2)
693             {
694                q1--;
695                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
696             } else {
697                q2--;
698                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
699             }
700             remaining_bits -= curr_bits;
701          }
702
703          if (q1 > 0) {
704             int spread = fold ? B : 0;
705             alg_quant(X, N, q1, spread, enc);
706          } else {
707             intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
708          }
709          if (q2 > 0) {
710             int spread = fold ? B : 0;
711             alg_quant(Y, N, q2, spread, enc);
712          } else
713             for (j=0;j<N;j++)
714                Y[j] = 0;
715       }
716       
717       balance += pulses[i] + tell;
718
719 #ifdef FIXED_POINT
720       mid = imid;
721       side = iside;
722 #else
723       mid = (1./32768)*imid;
724       side = (1./32768)*iside;
725 #endif
726       for (j=0;j<N;j++)
727          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
728
729       for (j=0;j<N;j++)
730          X[j] = MULT16_16_Q15(X[j], mid);
731       for (j=0;j<N;j++)
732          Y[j] = MULT16_16_Q15(Y[j], side);
733
734       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
735       renormalise_vector(X, Q15ONE, N, 1);
736       renormalise_vector(Y, Q15ONE, N, 1);
737    }
738    RESTORE_STACK;
739 }
740 #endif /* DISABLE_STEREO */
741
742
743 #ifndef DISABLE_STEREO
744
745 void unquant_bands_stereo(const CELTMode *m, celt_norm *_X, const celt_ener *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
746 {
747    int i, j, remaining_bits, balance;
748    const celt_int16 * restrict eBands = m->eBands;
749    celt_norm * restrict norm;
750    VARDECL(celt_norm, _norm);
751    int B;
752    celt_word16 mid, side;
753    SAVE_STACK;
754
755    B = shortBlocks ? m->nbShortMdcts : 1;
756    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm);
757    norm = _norm;
758
759    balance = 0;
760    for (i=0;i<m->nbEBands;i++)
761    {
762       int c;
763       int tell;
764       int q1, q2;
765       celt_word16 n;
766       const celt_int16 * const *BPbits;
767       int b, qb;
768       int N;
769       int curr_balance, curr_bits;
770       int imid, iside, itheta;
771       int mbits, sbits, delta;
772       int qalloc;
773       celt_norm * restrict X, * restrict Y;
774       
775       X = _X+eBands[i];
776       Y = X+eBands[m->nbEBands+1];
777       BPbits = m->bits;
778
779       N = eBands[i+1]-eBands[i];
780       tell = ec_dec_tell(dec, BITRES);
781       if (i != 0)
782          balance -= tell;
783       remaining_bits = (total_bits<<BITRES)-tell-1;
784       curr_balance = (m->nbEBands-i);
785       if (curr_balance > 3)
786          curr_balance = 3;
787       curr_balance = balance / curr_balance;
788       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
789       if (b<0)
790          b = 0;
791
792       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
793       if (qb > (b>>BITRES)-1)
794          qb = (b>>BITRES)-1;
795       if (qb>14)
796          qb = 14;
797       if (qb<0)
798          qb = 0;
799       qalloc = log2_frac((1<<qb)+1,BITRES);
800       if (qb==0)
801       {
802          itheta=0;
803       } else {
804          int shift;
805          shift = 14-qb;
806          itheta = ec_dec_uint(dec, (1<<qb)+1);
807          itheta <<= shift;
808       }
809       if (itheta == 0)
810       {
811          imid = 32767;
812          iside = 0;
813          delta = -10000;
814       } else if (itheta == 16384)
815       {
816          imid = 0;
817          iside = 32767;
818          delta = 10000;
819       } else {
820          imid = bitexact_cos(itheta);
821          iside = bitexact_cos(16384-itheta);
822          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
823       }
824       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
825
826       if (N==2)
827       {
828          int c2;
829          int sign=1;
830          celt_norm v[2], w[2];
831          celt_norm *x2, *y2;
832          mbits = b-qalloc;
833          sbits = 0;
834          if (itheta != 0 && itheta != 16384)
835             sbits = 1<<BITRES;
836          mbits -= sbits;
837          c = itheta > 8192 ? 1 : 0;
838          c2 = 1-c;
839
840          x2 = X;
841          y2 = Y;
842          v[0] = x2[c];
843          v[1] = y2[c];
844          w[0] = x2[c2];
845          w[1] = y2[c2];
846          q1 = bits2pulses(m, BPbits[i], N, mbits);
847          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
848          remaining_bits -= curr_bits;
849          while (remaining_bits < 0 && q1 > 0)
850          {
851             remaining_bits += curr_bits;
852             q1--;
853             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
854             remaining_bits -= curr_bits;
855          }
856
857          if (q1 > 0)
858          {
859             int spread = fold ? B : 0;
860             alg_unquant(v, N, q1, spread, dec);
861          } else {
862             v[0] = QCONST16(1.f, 14);
863             v[1] = 0;
864          }
865          if (sbits)
866             sign = 2*ec_dec_bits(dec, 1)-1;
867          else
868             sign = 1;
869          w[0] = -sign*v[1];
870          w[1] = sign*v[0];
871          if (c==0)
872          {
873             x2[0] = v[0];
874             x2[1] = v[1];
875             y2[0] = w[0];
876             y2[1] = w[1];
877          } else {
878             x2[0] = w[0];
879             x2[1] = w[1];
880             y2[0] = v[0];
881             y2[1] = v[1];
882          }
883       } else {
884          mbits = (b-qalloc/2-delta)/2;
885          if (mbits > b-qalloc)
886             mbits = b-qalloc;
887          if (mbits<0)
888             mbits=0;
889          sbits = b-qalloc-mbits;
890          q1 = bits2pulses(m, BPbits[i], N, mbits);
891          q2 = bits2pulses(m, BPbits[i], N, sbits);
892          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
893          remaining_bits -= curr_bits;
894          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
895          {
896             remaining_bits += curr_bits;
897             if (q1>q2)
898             {
899                q1--;
900                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
901             } else {
902                q2--;
903                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
904             }
905             remaining_bits -= curr_bits;
906          }
907          
908          if (q1 > 0)
909          {
910             int spread = fold ? B : 0;
911             alg_unquant(X, N, q1, spread, dec);
912          } else
913             intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
914          if (q2 > 0)
915          {
916             int spread = fold ? B : 0;
917             alg_unquant(Y, N, q2, spread, dec);
918          } else
919             for (j=0;j<N;j++)
920                Y[j] = 0;
921             /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
922       }
923       balance += pulses[i] + tell;
924       
925 #ifdef FIXED_POINT
926       mid = imid;
927       side = iside;
928 #else
929       mid = (1./32768)*imid;
930       side = (1./32768)*iside;
931 #endif
932       for (j=0;j<N;j++)
933          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
934
935       for (j=0;j<N;j++)
936          X[j] = MULT16_16_Q15(X[j], mid);
937       for (j=0;j<N;j++)
938          Y[j] = MULT16_16_Q15(Y[j], side);
939
940       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
941       renormalise_vector(X, Q15ONE, N, 1);
942       renormalise_vector(Y, Q15ONE, N, 1);
943    }
944    RESTORE_STACK;
945 }
946
947 #endif /* DISABLE_STEREO */