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