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