fixed-point: reducing the mismatch in the folded part
[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, int start, 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=start;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 != start)
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 = celt_sqrt(SHL32(EXTEND32(eBands[i+1]-eBands[i]),22));
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, start, 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, int start, 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=start;i<m->nbEBands;i++)
530    {
531       int tell;
532       int q1, q2;
533       celt_word16 n;
534       const celt_int16 * const *BPbits;
535       int b, qb;
536       int N;
537       int curr_balance, curr_bits;
538       int imid, iside, itheta;
539       int mbits, sbits, delta;
540       int qalloc;
541       celt_norm * restrict X, * restrict Y;
542       
543       X = _X+eBands[i];
544       Y = X+eBands[m->nbEBands+1];
545       BPbits = m->bits;
546
547       N = eBands[i+1]-eBands[i];
548       tell = ec_enc_tell(enc, BITRES);
549       if (i != start)
550          balance -= tell;
551       remaining_bits = (total_bits<<BITRES)-tell-1;
552       curr_balance = (m->nbEBands-i);
553       if (curr_balance > 3)
554          curr_balance = 3;
555       curr_balance = balance / curr_balance;
556       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
557       if (b<0)
558          b = 0;
559
560       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
561       if (qb > (b>>BITRES)-1)
562          qb = (b>>BITRES)-1;
563       if (qb<0)
564          qb = 0;
565       if (qb>14)
566          qb = 14;
567
568       stereo_band_mix(m, X, Y, bandE, qb==0, i, 1);
569
570       mid = renormalise_vector(X, Q15ONE, N, 1);
571       side = renormalise_vector(Y, Q15ONE, N, 1);
572 #ifdef FIXED_POINT
573       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
574 #else
575       itheta = floor(.5+16384*0.63662*atan2(side,mid));
576 #endif
577       qalloc = log2_frac((1<<qb)+1,BITRES);
578       if (qb==0)
579       {
580          itheta=0;
581       } else {
582          int shift;
583          shift = 14-qb;
584          itheta = (itheta+(1<<shift>>1))>>shift;
585          ec_enc_uint(enc, itheta, (1<<qb)+1);
586          itheta <<= shift;
587       }
588       if (itheta == 0)
589       {
590          imid = 32767;
591          iside = 0;
592          delta = -10000;
593       } else if (itheta == 16384)
594       {
595          imid = 0;
596          iside = 32767;
597          delta = 10000;
598       } else {
599          imid = bitexact_cos(itheta);
600          iside = bitexact_cos(16384-itheta);
601          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
602       }
603       n = celt_sqrt(SHL32(EXTEND32(eBands[i+1]-eBands[i]),22));
604 #if 0
605       if (N==2)
606       {
607          int c2;
608          int sign=1;
609          celt_norm v[2], w[2];
610          celt_norm *x2, *y2;
611          mbits = b-qalloc;
612          sbits = 0;
613          if (itheta != 0 && itheta != 16384)
614             sbits = 1<<BITRES;
615          mbits -= sbits;
616          c = itheta > 8192 ? 1 : 0;
617          c2 = 1-c;
618
619          x2 = X;
620          y2 = Y;
621          if (c==0)
622          {
623             v[0] = x2[0];
624             v[1] = x2[1];
625             w[0] = y2[0];
626             w[1] = y2[1];
627          } else {
628             v[0] = y2[0];
629             v[1] = y2[1];
630             w[0] = x2[0];
631             w[1] = x2[1];
632          }
633          q1 = bits2pulses(m, BPbits[i], N, mbits);
634          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
635          remaining_bits -= curr_bits;
636          while (remaining_bits < 0 && q1 > 0)
637          {
638             remaining_bits += curr_bits;
639             q1--;
640             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
641             remaining_bits -= curr_bits;
642          }
643
644          if (q1 > 0)
645          {
646             int spread = fold ? B : 0;
647             alg_quant(v, N, q1, spread, enc);
648          } else {
649             v[0] = QCONST16(1.f, 14);
650             v[1] = 0;
651          }
652          if (sbits)
653          {
654             if (v[0]*w[1] - v[1]*w[0] > 0)
655                sign = 1;
656             else
657                sign = -1;
658             ec_enc_bits(enc, sign==1, 1);
659          } else {
660             sign = 1;
661          }
662          w[0] = -sign*v[1];
663          w[1] = sign*v[0];
664          if (c==0)
665          {
666             x2[0] = v[0];
667             x2[1] = v[1];
668             y2[0] = w[0];
669             y2[1] = w[1];
670          } else {
671             x2[0] = w[0];
672             x2[1] = w[1];
673             y2[0] = v[0];
674             y2[1] = v[1];
675          }
676       } else 
677 #endif
678       {
679          
680          mbits = (b-qalloc/2-delta)/2;
681          if (mbits > b-qalloc)
682             mbits = b-qalloc;
683          if (mbits<0)
684             mbits=0;
685          sbits = b-qalloc-mbits;
686          q1 = bits2pulses(m, BPbits[i], N, mbits);
687          q2 = bits2pulses(m, BPbits[i], N, sbits);
688          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
689          remaining_bits -= curr_bits;
690          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
691          {
692             remaining_bits += curr_bits;
693             if (q1>q2)
694             {
695                q1--;
696                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
697             } else {
698                q2--;
699                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
700             }
701             remaining_bits -= curr_bits;
702          }
703
704          if (q1 > 0) {
705             int spread = fold ? B : 0;
706             alg_quant(X, N, q1, spread, enc);
707          } else {
708             intra_fold(m, start, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
709          }
710          if (q2 > 0) {
711             int spread = fold ? B : 0;
712             alg_quant(Y, N, q2, spread, enc);
713          } else
714             for (j=0;j<N;j++)
715                Y[j] = 0;
716       }
717       
718       balance += pulses[i] + tell;
719
720 #ifdef FIXED_POINT
721       mid = imid;
722       side = iside;
723 #else
724       mid = (1./32768)*imid;
725       side = (1./32768)*iside;
726 #endif
727       for (j=0;j<N;j++)
728          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
729
730       for (j=0;j<N;j++)
731          X[j] = MULT16_16_Q15(X[j], mid);
732       for (j=0;j<N;j++)
733          Y[j] = MULT16_16_Q15(Y[j], side);
734
735       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
736       renormalise_vector(X, Q15ONE, N, 1);
737       renormalise_vector(Y, Q15ONE, N, 1);
738    }
739    RESTORE_STACK;
740 }
741 #endif /* DISABLE_STEREO */
742
743
744 #ifndef DISABLE_STEREO
745
746 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)
747 {
748    int i, j, remaining_bits, balance;
749    const celt_int16 * restrict eBands = m->eBands;
750    celt_norm * restrict norm;
751    VARDECL(celt_norm, _norm);
752    int B;
753    celt_word16 mid, side;
754    SAVE_STACK;
755
756    B = shortBlocks ? m->nbShortMdcts : 1;
757    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm);
758    norm = _norm;
759
760    balance = 0;
761    for (i=start;i<m->nbEBands;i++)
762    {
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 != start)
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 = celt_sqrt(SHL32(EXTEND32(eBands[i+1]-eBands[i]),22));
825
826 #if 0
827       if (N==2)
828       {
829          int c2;
830          int sign=1;
831          celt_norm v[2], w[2];
832          celt_norm *x2, *y2;
833          mbits = b-qalloc;
834          sbits = 0;
835          if (itheta != 0 && itheta != 16384)
836             sbits = 1<<BITRES;
837          mbits -= sbits;
838          c = itheta > 8192 ? 1 : 0;
839          c2 = 1-c;
840
841          x2 = X;
842          y2 = Y;
843          v[0] = x2[c];
844          v[1] = y2[c];
845          w[0] = x2[c2];
846          w[1] = y2[c2];
847          q1 = bits2pulses(m, BPbits[i], N, mbits);
848          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
849          remaining_bits -= curr_bits;
850          while (remaining_bits < 0 && q1 > 0)
851          {
852             remaining_bits += curr_bits;
853             q1--;
854             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
855             remaining_bits -= curr_bits;
856          }
857
858          if (q1 > 0)
859          {
860             int spread = fold ? B : 0;
861             alg_unquant(v, N, q1, spread, dec);
862          } else {
863             v[0] = QCONST16(1.f, 14);
864             v[1] = 0;
865          }
866          if (sbits)
867             sign = 2*ec_dec_bits(dec, 1)-1;
868          else
869             sign = 1;
870          w[0] = -sign*v[1];
871          w[1] = sign*v[0];
872          if (c==0)
873          {
874             x2[0] = v[0];
875             x2[1] = v[1];
876             y2[0] = w[0];
877             y2[1] = w[1];
878          } else {
879             x2[0] = w[0];
880             x2[1] = w[1];
881             y2[0] = v[0];
882             y2[1] = v[1];
883          }
884       } else
885 #endif
886       {
887          mbits = (b-qalloc/2-delta)/2;
888          if (mbits > b-qalloc)
889             mbits = b-qalloc;
890          if (mbits<0)
891             mbits=0;
892          sbits = b-qalloc-mbits;
893          q1 = bits2pulses(m, BPbits[i], N, mbits);
894          q2 = bits2pulses(m, BPbits[i], N, sbits);
895          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
896          remaining_bits -= curr_bits;
897          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
898          {
899             remaining_bits += curr_bits;
900             if (q1>q2)
901             {
902                q1--;
903                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
904             } else {
905                q2--;
906                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
907             }
908             remaining_bits -= curr_bits;
909          }
910          
911          if (q1 > 0)
912          {
913             int spread = fold ? B : 0;
914             alg_unquant(X, N, q1, spread, dec);
915          } else
916             intra_fold(m, start, eBands[i+1]-eBands[i], norm, X, eBands[i], B);
917          if (q2 > 0)
918          {
919             int spread = fold ? B : 0;
920             alg_unquant(Y, N, q2, spread, dec);
921          } else
922             for (j=0;j<N;j++)
923                Y[j] = 0;
924             /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
925       }
926       balance += pulses[i] + tell;
927       
928 #ifdef FIXED_POINT
929       mid = imid;
930       side = iside;
931 #else
932       mid = (1./32768)*imid;
933       side = (1./32768)*iside;
934 #endif
935       for (j=0;j<N;j++)
936          norm[eBands[i]+j] = MULT16_16_Q15(n,X[j]);
937
938       for (j=0;j<N;j++)
939          X[j] = MULT16_16_Q15(X[j], mid);
940       for (j=0;j<N;j++)
941          Y[j] = MULT16_16_Q15(Y[j], side);
942
943       stereo_band_mix(m, X, Y, bandE, 0, i, -1);
944       renormalise_vector(X, Q15ONE, N, 1);
945       renormalise_vector(Y, Q15ONE, N, 1);
946    }
947    RESTORE_STACK;
948 }
949
950 #endif /* DISABLE_STEREO */