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