Removed the _t from all the celt*_t types to avoid clashing with POSIX
[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_ilog2(Sxy)-16;
266       if (shift < 0)
267          shift = 0;
268       g = DIV32(SHL32(SHR32(num,shift),14),SHR32(den,shift));
269       if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
270          g = 0;
271       /* This MUST round down so that we don't over-estimate the gain */
272       *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
273    }
274 #else
275    {
276       float fact = .04*norm_rate;
277       if (fact < 1)
278          fact = 1;
279       g = Sxy/(.1+Sxx+.03*Syy);
280       if (Sxy < .5*fact*celt_sqrt(1+Sxx*Syy))
281          g = 0;
282       /* This MUST round down so that we don't over-estimate the gain */
283       *gain_id = floor(20*(g-.5));
284    }
285 #endif
286    /* This prevents the pitch gain from being above 1.0 for too long by bounding the 
287       maximum error amplification factor to 2.0 */
288    g = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),*gain_id));
289    *gain_prod = MAX16(QCONST32(1., 13), MULT16_16_Q14(*gain_prod,g));
290    if (*gain_prod>QCONST32(2., 13))
291    {
292       *gain_id=9;
293       *gain_prod = QCONST32(2., 13);
294    }
295
296    if (*gain_id < 0)
297    {
298       *gain_id = 0;
299       return 0;
300    } else {
301       if (*gain_id > 15)
302          *gain_id = 15;
303       return 1;
304    }
305 }
306
307 void apply_pitch(const CELTMode *m, celt_sig *X, const celt_sig *P, int gain_id, int pred, int _C)
308 {
309    int j, c, N;
310    celt_word16 gain;
311    celt_word16 delta;
312    const int C = CHANNELS(_C);
313    int len = m->pitchEnd;
314
315    N = FRAMESIZE(m);
316    gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
317    delta = PDIV32_16(gain, len);
318    if (pred)
319       gain = -gain;
320    else
321       delta = -delta;
322    for (c=0;c<C;c++)
323    {
324       celt_word16 gg = gain;
325       for (j=0;j<len;j++)
326       {
327          X[j+c*N] += SHL32(MULT16_32_Q15(gg,P[j+c*N]),1);
328          gg = ADD16(gg, delta);
329       }
330    }
331 }
332
333 #ifndef DISABLE_STEREO
334
335 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)
336 {
337    int i = bandID;
338    const celt_int16 *eBands = m->eBands;
339    int j;
340    celt_word16 a1, a2;
341    if (stereo_mode==0)
342    {
343       /* Do mid-side when not doing intensity stereo */
344       a1 = QCONST16(.70711f,14);
345       a2 = dir*QCONST16(.70711f,14);
346    } else {
347       celt_word16 left, right;
348       celt_word16 norm;
349 #ifdef FIXED_POINT
350       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
351 #endif
352       left = VSHR32(bank[i],shift);
353       right = VSHR32(bank[i+m->nbEBands],shift);
354       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
355       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
356       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
357    }
358    for (j=0;j<eBands[i+1]-eBands[i];j++)
359    {
360       celt_norm r, l;
361       l = X[j];
362       r = Y[j];
363       X[j] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
364       Y[j] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
365    }
366 }
367
368
369 #endif /* DISABLE_STEREO */
370
371 int folding_decision(const CELTMode *m, celt_norm *X, celt_word16 *average, int *last_decision, int _C)
372 {
373    int i, c, N0;
374    int NR=0;
375    celt_word32 ratio = EPSILON;
376    const int C = CHANNELS(_C);
377    const celt_int16 * restrict eBands = m->eBands;
378    
379    N0 = FRAMESIZE(m);
380
381    for (c=0;c<C;c++)
382    {
383    for (i=0;i<m->nbEBands;i++)
384    {
385       int j, N;
386       int max_i=0;
387       celt_word16 max_val=EPSILON;
388       celt_word32 floor_ener=EPSILON;
389       celt_norm * restrict x = X+eBands[i]+c*N0;
390       N = eBands[i+1]-eBands[i];
391       for (j=0;j<N;j++)
392       {
393          if (ABS16(x[j])>max_val)
394          {
395             max_val = ABS16(x[j]);
396             max_i = j;
397          }
398       }
399 #if 0
400       for (j=0;j<N;j++)
401       {
402          if (abs(j-max_i)>2)
403             floor_ener += x[j]*x[j];
404       }
405 #else
406       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
407       if (max_i < N-1)
408          floor_ener -= MULT16_16(x[(max_i+1)], x[(max_i+1)]);
409       if (max_i < N-2)
410          floor_ener -= MULT16_16(x[(max_i+2)], x[(max_i+2)]);
411       if (max_i > 0)
412          floor_ener -= MULT16_16(x[(max_i-1)], x[(max_i-1)]);
413       if (max_i > 1)
414          floor_ener -= MULT16_16(x[(max_i-2)], x[(max_i-2)]);
415       floor_ener = MAX32(floor_ener, EPSILON);
416 #endif
417       if (N>7)
418       {
419          celt_word16 r;
420          celt_word16 den = celt_sqrt(floor_ener);
421          den = MAX32(QCONST16(.02, 15), den);
422          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
423          ratio = ADD32(ratio, EXTEND32(r));
424          NR++;
425       }
426    }
427    }
428    if (NR>0)
429       ratio = DIV32_16(ratio, NR);
430    ratio = ADD32(HALF32(ratio), HALF32(*average));
431    if (!*last_decision)
432    {
433       *last_decision = (ratio < QCONST16(1.8,8));
434    } else {
435       *last_decision = (ratio < QCONST16(3.,8));
436    }
437    *average = EXTRACT16(ratio);
438    return *last_decision;
439 }
440
441 /* Quantisation of the residual */
442 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)
443 {
444    int i, j, remaining_bits, balance;
445    const celt_int16 * restrict eBands = m->eBands;
446    celt_norm * restrict norm;
447    VARDECL(celt_norm, _norm);
448    int B;
449    SAVE_STACK;
450
451    B = shortBlocks ? m->nbShortMdcts : 1;
452    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm);
453    norm = _norm;
454
455    balance = 0;
456    for (i=0;i<m->nbEBands;i++)
457    {
458       int tell;
459       int N;
460       int q;
461       celt_word16 n;
462       const celt_int16 * const *BPbits;
463       
464       int curr_balance, curr_bits;
465       
466       N = eBands[i+1]-eBands[i];
467       BPbits = m->bits;
468
469       if (encode)
470          tell = ec_enc_tell(enc_dec, BITRES);
471       else
472          tell = ec_dec_tell(enc_dec, BITRES);
473       if (i != 0)
474          balance -= tell;
475       remaining_bits = (total_bits<<BITRES)-tell-1;
476       curr_balance = (m->nbEBands-i);
477       if (curr_balance > 3)
478          curr_balance = 3;
479       curr_balance = balance / curr_balance;
480       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
481       curr_bits = pulses2bits(BPbits[i], N, q);
482       remaining_bits -= curr_bits;
483       while (remaining_bits < 0 && q > 0)
484       {
485          remaining_bits += curr_bits;
486          q--;
487          curr_bits = pulses2bits(BPbits[i], N, q);
488          remaining_bits -= curr_bits;
489       }
490       balance += pulses[i] + tell;
491       
492       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
493
494       if (q > 0)
495       {
496          int spread = fold ? B : 0;
497          if (encode)
498             alg_quant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc_dec);
499          else
500             alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc_dec);
501       } else {
502          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
503       }
504       for (j=eBands[i];j<eBands[i+1];j++)
505          norm[j] = MULT16_16_Q15(n,X[j]);
506    }
507    RESTORE_STACK;
508 }
509
510 #ifndef DISABLE_STEREO
511
512 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)
513 {
514    int i, j, remaining_bits, balance;
515    const celt_int16 * restrict eBands = m->eBands;
516    celt_norm * restrict norm;
517    VARDECL(celt_norm, _norm);
518    int B;
519    celt_word16 mid, side;
520    SAVE_STACK;
521
522    B = shortBlocks ? m->nbShortMdcts : 1;
523    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm);
524    norm = _norm;
525
526    balance = 0;
527    for (i=0;i<m->nbEBands;i++)
528    {
529       int c;
530       int tell;
531       int q1, q2;
532       celt_word16 n;
533       const celt_int16 * const *BPbits;
534       int b, qb;
535       int N;
536       int curr_balance, curr_bits;
537       int imid, iside, itheta;
538       int mbits, sbits, delta;
539       int qalloc;
540       celt_norm * restrict X, * restrict Y;
541       
542       X = _X+eBands[i];
543       Y = X+eBands[m->nbEBands+1];
544       BPbits = m->bits;
545
546       N = eBands[i+1]-eBands[i];
547       tell = ec_enc_tell(enc, BITRES);
548       if (i != 0)
549          balance -= tell;
550       remaining_bits = (total_bits<<BITRES)-tell-1;
551       curr_balance = (m->nbEBands-i);
552       if (curr_balance > 3)
553          curr_balance = 3;
554       curr_balance = balance / curr_balance;
555       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
556       if (b<0)
557          b = 0;
558
559       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
560       if (qb > (b>>BITRES)-1)
561          qb = (b>>BITRES)-1;
562       if (qb<0)
563          qb = 0;
564       if (qb>14)
565          qb = 14;
566
567       stereo_band_mix(m, X, Y, bandE, qb==0, i, 1);
568
569       mid = renormalise_vector(X, Q15ONE, N, 1);
570       side = renormalise_vector(Y, Q15ONE, N, 1);
571 #ifdef FIXED_POINT
572       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
573 #else
574       itheta = floor(.5+16384*0.63662*atan2(side,mid));
575 #endif
576       qalloc = log2_frac((1<<qb)+1,BITRES);
577       if (qb==0)
578       {
579          itheta=0;
580       } else {
581          int shift;
582          shift = 14-qb;
583          itheta = (itheta+(1<<shift>>1))>>shift;
584          ec_enc_uint(enc, itheta, (1<<qb)+1);
585          itheta <<= shift;
586       }
587       if (itheta == 0)
588       {
589          imid = 32767;
590          iside = 0;
591          delta = -10000;
592       } else if (itheta == 16384)
593       {
594          imid = 0;
595          iside = 32767;
596          delta = 10000;
597       } else {
598          imid = bitexact_cos(itheta);
599          iside = bitexact_cos(16384-itheta);
600          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
601       }
602       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
603
604       if (N==2)
605       {
606          int c2;
607          int sign=1;
608          celt_norm v[2], w[2];
609          celt_norm *x2, *y2;
610          mbits = b-qalloc;
611          sbits = 0;
612          if (itheta != 0 && itheta != 16384)
613             sbits = 1<<BITRES;
614          mbits -= sbits;
615          c = itheta > 8192 ? 1 : 0;
616          c2 = 1-c;
617
618          x2 = X;
619          y2 = Y;
620          if (c==0)
621          {
622             v[0] = x2[0];
623             v[1] = x2[1];
624             w[0] = y2[0];
625             w[1] = y2[1];
626          } else {
627             v[0] = y2[0];
628             v[1] = y2[1];
629             w[0] = x2[0];
630             w[1] = x2[1];
631          }
632          q1 = bits2pulses(m, BPbits[i], N, mbits);
633          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
634          remaining_bits -= curr_bits;
635          while (remaining_bits < 0 && q1 > 0)
636          {
637             remaining_bits += curr_bits;
638             q1--;
639             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
640             remaining_bits -= curr_bits;
641          }
642
643          if (q1 > 0)
644          {
645             int spread = fold ? B : 0;
646             alg_quant(v, N, q1, spread, enc);
647          } else {
648             v[0] = QCONST16(1.f, 14);
649             v[1] = 0;
650          }
651          if (sbits)
652          {
653             if (v[0]*w[1] - v[1]*w[0] > 0)
654                sign = 1;
655             else
656                sign = -1;
657             ec_enc_bits(enc, sign==1, 1);
658          } else {
659             sign = 1;
660          }
661          w[0] = -sign*v[1];
662          w[1] = sign*v[0];
663          if (c==0)
664          {
665             x2[0] = v[0];
666             x2[1] = v[1];
667             y2[0] = w[0];
668             y2[1] = w[1];
669          } else {
670             x2[0] = w[0];
671             x2[1] = w[1];
672             y2[0] = v[0];
673             y2[1] = v[1];
674          }
675       } else {
676          
677          mbits = (b-qalloc/2-delta)/2;
678          if (mbits > b-qalloc)
679             mbits = b-qalloc;
680          if (mbits<0)
681             mbits=0;
682          sbits = b-qalloc-mbits;
683          q1 = bits2pulses(m, BPbits[i], N, mbits);
684          q2 = bits2pulses(m, BPbits[i], N, sbits);
685          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
686          remaining_bits -= curr_bits;
687          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
688          {
689             remaining_bits += curr_bits;
690             if (q1>q2)
691             {
692                q1--;
693                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
694             } else {
695                q2--;
696                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
697             }
698             remaining_bits -= curr_bits;
699          }
700
701          if (q1 > 0) {
702             int spread = fold ? B : 0;
703             alg_quant(X, N, q1, spread, enc);
704          } else {
705             intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
706          }
707          if (q2 > 0) {
708             int spread = fold ? B : 0;
709             alg_quant(Y, N, q2, spread, enc);
710          } else
711             for (j=0;j<N;j++)
712                Y[j] = 0;
713       }
714       
715       balance += pulses[i] + tell;
716
717 #ifdef FIXED_POINT
718       mid = imid;
719       side = iside;
720 #else
721       mid = (1./32768)*imid;
722       side = (1./32768)*iside;
723 #endif
724       for (j=0;j<N;j++)
725          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
726
727       for (j=0;j<N;j++)
728          X[j] = MULT16_16_Q15(X[j], mid);
729       for (j=0;j<N;j++)
730          Y[j] = MULT16_16_Q15(Y[j], side);
731
732       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
733       renormalise_vector(X, Q15ONE, N, 1);
734       renormalise_vector(Y, Q15ONE, N, 1);
735    }
736    RESTORE_STACK;
737 }
738 #endif /* DISABLE_STEREO */
739
740
741 #ifndef DISABLE_STEREO
742
743 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)
744 {
745    int i, j, remaining_bits, balance;
746    const celt_int16 * restrict eBands = m->eBands;
747    celt_norm * restrict norm;
748    VARDECL(celt_norm, _norm);
749    int B;
750    celt_word16 mid, side;
751    SAVE_STACK;
752
753    B = shortBlocks ? m->nbShortMdcts : 1;
754    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm);
755    norm = _norm;
756
757    balance = 0;
758    for (i=0;i<m->nbEBands;i++)
759    {
760       int c;
761       int tell;
762       int q1, q2;
763       celt_word16 n;
764       const celt_int16 * const *BPbits;
765       int b, qb;
766       int N;
767       int curr_balance, curr_bits;
768       int imid, iside, itheta;
769       int mbits, sbits, delta;
770       int qalloc;
771       celt_norm * restrict X, * restrict Y;
772       
773       X = _X+eBands[i];
774       Y = X+eBands[m->nbEBands+1];
775       BPbits = m->bits;
776
777       N = eBands[i+1]-eBands[i];
778       tell = ec_dec_tell(dec, BITRES);
779       if (i != 0)
780          balance -= tell;
781       remaining_bits = (total_bits<<BITRES)-tell-1;
782       curr_balance = (m->nbEBands-i);
783       if (curr_balance > 3)
784          curr_balance = 3;
785       curr_balance = balance / curr_balance;
786       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
787       if (b<0)
788          b = 0;
789
790       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
791       if (qb > (b>>BITRES)-1)
792          qb = (b>>BITRES)-1;
793       if (qb>14)
794          qb = 14;
795       if (qb<0)
796          qb = 0;
797       qalloc = log2_frac((1<<qb)+1,BITRES);
798       if (qb==0)
799       {
800          itheta=0;
801       } else {
802          int shift;
803          shift = 14-qb;
804          itheta = ec_dec_uint(dec, (1<<qb)+1);
805          itheta <<= shift;
806       }
807       if (itheta == 0)
808       {
809          imid = 32767;
810          iside = 0;
811          delta = -10000;
812       } else if (itheta == 16384)
813       {
814          imid = 0;
815          iside = 32767;
816          delta = 10000;
817       } else {
818          imid = bitexact_cos(itheta);
819          iside = bitexact_cos(16384-itheta);
820          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
821       }
822       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
823
824       if (N==2)
825       {
826          int c2;
827          int sign=1;
828          celt_norm v[2], w[2];
829          celt_norm *x2, *y2;
830          mbits = b-qalloc;
831          sbits = 0;
832          if (itheta != 0 && itheta != 16384)
833             sbits = 1<<BITRES;
834          mbits -= sbits;
835          c = itheta > 8192 ? 1 : 0;
836          c2 = 1-c;
837
838          x2 = X;
839          y2 = Y;
840          v[0] = x2[c];
841          v[1] = y2[c];
842          w[0] = x2[c2];
843          w[1] = y2[c2];
844          q1 = bits2pulses(m, BPbits[i], N, mbits);
845          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
846          remaining_bits -= curr_bits;
847          while (remaining_bits < 0 && q1 > 0)
848          {
849             remaining_bits += curr_bits;
850             q1--;
851             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
852             remaining_bits -= curr_bits;
853          }
854
855          if (q1 > 0)
856          {
857             int spread = fold ? B : 0;
858             alg_unquant(v, N, q1, spread, dec);
859          } else {
860             v[0] = QCONST16(1.f, 14);
861             v[1] = 0;
862          }
863          if (sbits)
864             sign = 2*ec_dec_bits(dec, 1)-1;
865          else
866             sign = 1;
867          w[0] = -sign*v[1];
868          w[1] = sign*v[0];
869          if (c==0)
870          {
871             x2[0] = v[0];
872             x2[1] = v[1];
873             y2[0] = w[0];
874             y2[1] = w[1];
875          } else {
876             x2[0] = w[0];
877             x2[1] = w[1];
878             y2[0] = v[0];
879             y2[1] = v[1];
880          }
881       } else {
882          mbits = (b-qalloc/2-delta)/2;
883          if (mbits > b-qalloc)
884             mbits = b-qalloc;
885          if (mbits<0)
886             mbits=0;
887          sbits = b-qalloc-mbits;
888          q1 = bits2pulses(m, BPbits[i], N, mbits);
889          q2 = bits2pulses(m, BPbits[i], N, sbits);
890          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
891          remaining_bits -= curr_bits;
892          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
893          {
894             remaining_bits += curr_bits;
895             if (q1>q2)
896             {
897                q1--;
898                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
899             } else {
900                q2--;
901                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
902             }
903             remaining_bits -= curr_bits;
904          }
905          
906          if (q1 > 0)
907          {
908             int spread = fold ? B : 0;
909             alg_unquant(X, N, q1, spread, dec);
910          } else
911             intra_fold(m, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
912          if (q2 > 0)
913          {
914             int spread = fold ? B : 0;
915             alg_unquant(Y, N, q2, spread, dec);
916          } else
917             for (j=0;j<N;j++)
918                Y[j] = 0;
919             /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
920       }
921       balance += pulses[i] + tell;
922       
923 #ifdef FIXED_POINT
924       mid = imid;
925       side = iside;
926 #else
927       mid = (1./32768)*imid;
928       side = (1./32768)*iside;
929 #endif
930       for (j=0;j<N;j++)
931          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
932
933       for (j=0;j<N;j++)
934          X[j] = MULT16_16_Q15(X[j], mid);
935       for (j=0;j<N;j++)
936          Y[j] = MULT16_16_Q15(Y[j], side);
937
938       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
939       renormalise_vector(X, Q15ONE, N, 1);
940       renormalise_vector(Y, Q15ONE, N, 1);
941    }
942    RESTORE_STACK;
943 }
944
945 #endif /* DISABLE_STEREO */