Fix stereo mismatch problem
[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;
381    int NR=0;
382    celt_word32_t ratio = EPSILON;
383    const celt_int16_t * restrict eBands = m->eBands;
384    for (i=0;i<m->nbEBands;i++)
385    {
386       int j, N;
387       int max_i=0;
388       celt_word16_t max_val=EPSILON;
389       celt_word32_t floor_ener=EPSILON;
390       celt_norm_t * restrict x = X+eBands[i];
391       N = eBands[i+1]-eBands[i];
392       for (j=0;j<N;j++)
393       {
394          if (ABS16(x[j])>max_val)
395          {
396             max_val = ABS16(x[j]);
397             max_i = j;
398          }
399       }
400 #if 0
401       for (j=0;j<N;j++)
402       {
403          if (abs(j-max_i)>2)
404             floor_ener += x[j]*x[j];
405       }
406 #else
407       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
408       if (max_i < N-1)
409          floor_ener -= MULT16_16(x[max_i+1], x[max_i+1]);
410       if (max_i < N-2)
411          floor_ener -= MULT16_16(x[max_i+2], x[max_i+2]);
412       if (max_i > 0)
413          floor_ener -= MULT16_16(x[max_i-1], x[max_i-1]);
414       if (max_i > 1)
415          floor_ener -= MULT16_16(x[max_i-2], x[max_i-2]);
416       floor_ener = MAX32(floor_ener, EPSILON);
417 #endif
418       if (N>7)
419       {
420          celt_word16_t r;
421          celt_word16_t den = celt_sqrt(floor_ener);
422          den = MAX32(QCONST16(.02, 15), den);
423          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
424          ratio = ADD32(ratio, EXTEND32(r));
425          NR++;
426       }
427    }
428    if (NR>0)
429       ratio = DIV32_16(ratio, NR);
430    ratio = ADD32(HALF32(ratio), HALF32(*average));
431    if (!*last_decision)
432    {
433       *last_decision = (ratio < QCONST16(1.8,8));
434    } else {
435       *last_decision = (ratio < QCONST16(3.,8));
436    }
437    *average = EXTRACT16(ratio);
438    return *last_decision;
439 }
440
441 /* Quantisation of the residual */
442 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
443 {
444    int i, j, remaining_bits, balance;
445    const celt_int16_t * restrict eBands = m->eBands;
446    celt_norm_t * restrict norm;
447    VARDECL(celt_norm_t, _norm);
448    int B;
449    SAVE_STACK;
450
451    B = shortBlocks ? m->nbShortMdcts : 1;
452    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
453    norm = _norm;
454
455    balance = 0;
456    for (i=0;i<m->nbEBands;i++)
457    {
458       int tell;
459       int N;
460       int q;
461       celt_word16_t n;
462       const celt_int16_t * const *BPbits;
463       
464       int curr_balance, curr_bits;
465       
466       N = eBands[i+1]-eBands[i];
467       BPbits = m->bits;
468
469       tell = ec_enc_tell(enc, BITRES);
470       if (i != 0)
471          balance -= tell;
472       remaining_bits = (total_bits<<BITRES)-tell-1;
473       curr_balance = (m->nbEBands-i);
474       if (curr_balance > 3)
475          curr_balance = 3;
476       curr_balance = balance / curr_balance;
477       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
478       curr_bits = pulses2bits(BPbits[i], N, q);
479       remaining_bits -= curr_bits;
480       while (remaining_bits < 0 && q > 0)
481       {
482          remaining_bits += curr_bits;
483          q--;
484          curr_bits = pulses2bits(BPbits[i], N, q);
485          remaining_bits -= curr_bits;
486       }
487       balance += pulses[i] + tell;
488       
489       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
490
491       if (q > 0)
492       {
493          int spread = fold ? B : 0;
494          alg_quant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc);
495       } else {
496          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
497       }
498       for (j=eBands[i];j<eBands[i+1];j++)
499          norm[j] = MULT16_16_Q15(n,X[j]);
500    }
501    RESTORE_STACK;
502 }
503
504 #ifndef DISABLE_STEREO
505
506 void quant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
507 {
508    int i, j, remaining_bits, balance;
509    const celt_int16_t * restrict eBands = m->eBands;
510    celt_norm_t * restrict norm;
511    VARDECL(celt_norm_t, _norm);
512    const int C = CHANNELS(m);
513    int B;
514    celt_word16_t mid, side;
515    SAVE_STACK;
516
517    B = shortBlocks ? m->nbShortMdcts : 1;
518    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
519    norm = _norm;
520
521    balance = 0;
522    for (i=0;i<m->nbEBands;i++)
523    {
524       int c;
525       int tell;
526       int q1, q2;
527       celt_word16_t n;
528       const celt_int16_t * const *BPbits;
529       int b, qb;
530       int N;
531       int curr_balance, curr_bits;
532       int imid, iside, itheta;
533       int mbits, sbits, delta;
534       int qalloc;
535       
536       BPbits = m->bits;
537
538       N = eBands[i+1]-eBands[i];
539       tell = ec_enc_tell(enc, BITRES);
540       if (i != 0)
541          balance -= tell;
542       remaining_bits = (total_bits<<BITRES)-tell-1;
543       curr_balance = (m->nbEBands-i);
544       if (curr_balance > 3)
545          curr_balance = 3;
546       curr_balance = balance / curr_balance;
547       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
548       if (b<0)
549          b = 0;
550
551       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
552       if (qb > (b>>BITRES)-1)
553          qb = (b>>BITRES)-1;
554       if (qb<0)
555          qb = 0;
556       if (qb>14)
557          qb = 14;
558
559       stereo_band_mix(m, X, bandE, qb==0, i, 1);
560
561       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
562       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
563 #ifdef FIXED_POINT
564       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
565 #else
566       itheta = floor(.5+16384*0.63662*atan2(side,mid));
567 #endif
568       qalloc = log2_frac((1<<qb)+1,BITRES);
569       if (qb==0)
570       {
571          itheta=0;
572       } else {
573          int shift;
574          shift = 14-qb;
575          itheta = (itheta+(1<<shift>>1))>>shift;
576          ec_enc_uint(enc, itheta, (1<<qb)+1);
577          itheta <<= shift;
578       }
579       if (itheta == 0)
580       {
581          imid = 32767;
582          iside = 0;
583          delta = -10000;
584       } else if (itheta == 16384)
585       {
586          imid = 0;
587          iside = 32767;
588          delta = 10000;
589       } else {
590          imid = bitexact_cos(itheta);
591          iside = bitexact_cos(16384-itheta);
592          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
593       }
594       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
595
596       if (N==2)
597       {
598          int c2;
599          int sign=1;
600          celt_norm_t v[2], w[2];
601          celt_norm_t *x2 = X+C*eBands[i];
602          mbits = b-qalloc;
603          sbits = 0;
604          if (itheta != 0 && itheta != 16384)
605             sbits = 1<<BITRES;
606          mbits -= sbits;
607          c = itheta > 8192 ? 1 : 0;
608          c2 = 1-c;
609
610          v[0] = x2[c];
611          v[1] = x2[c+C];
612          w[0] = x2[c2];
613          w[1] = x2[c2+C];
614          q1 = bits2pulses(m, BPbits[i], N, mbits);
615          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
616          remaining_bits -= curr_bits;
617          while (remaining_bits < 0 && q1 > 0)
618          {
619             remaining_bits += curr_bits;
620             q1--;
621             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
622             remaining_bits -= curr_bits;
623          }
624
625          if (q1 > 0)
626          {
627             int spread = fold ? B : 0;
628             alg_quant(v, N, q1, spread, enc);
629          } else {
630             v[0] = QCONST16(1.f, 14);
631             v[1] = 0;
632          }
633          if (sbits)
634          {
635             if (v[0]*w[1] - v[1]*w[0] > 0)
636                sign = 1;
637             else
638                sign = -1;
639             ec_enc_bits(enc, sign==1, 1);
640          } else {
641             sign = 1;
642          }
643          w[0] = -sign*v[1];
644          w[1] = sign*v[0];
645          if (c==0)
646          {
647             x2[0] = v[0];
648             x2[1] = v[1];
649             x2[2] = w[0];
650             x2[3] = w[1];
651          } else {
652             x2[0] = w[0];
653             x2[1] = w[1];
654             x2[2] = v[0];
655             x2[3] = v[1];
656          }
657       } else {
658          
659       mbits = (b-qalloc/2-delta)/2;
660       if (mbits > b-qalloc)
661          mbits = b-qalloc;
662       if (mbits<0)
663          mbits=0;
664       sbits = b-qalloc-mbits;
665       q1 = bits2pulses(m, BPbits[i], N, mbits);
666       q2 = bits2pulses(m, BPbits[i], N, sbits);
667       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
668       remaining_bits -= curr_bits;
669       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
670       {
671          remaining_bits += curr_bits;
672          if (q1>q2)
673          {
674             q1--;
675             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
676          } else {
677             q2--;
678             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
679          }
680          remaining_bits -= curr_bits;
681       }
682
683       /* If pitch isn't available, use intra-frame prediction */
684       if (q1==0)
685       {
686          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
687          deinterleave(P+C*eBands[i], C*N);
688       }
689       deinterleave(X+C*eBands[i], C*N);
690       if (q1 > 0) {
691          int spread = fold ? B : 0;
692          alg_quant(X+C*eBands[i], N, q1, spread, enc);
693       } else
694          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
695             X[j] = P[j];
696       if (q2 > 0) {
697          int spread = fold ? B : 0;
698          alg_quant(X+C*eBands[i]+N, N, q2, spread, enc);
699       } else
700          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
701             X[j] = 0;
702       }
703       
704       balance += pulses[i] + tell;
705
706 #ifdef FIXED_POINT
707       mid = imid;
708       side = iside;
709 #else
710       mid = (1./32768)*imid;
711       side = (1./32768)*iside;
712 #endif
713       for (c=0;c<C;c++)
714          for (j=0;j<N;j++)
715             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
716
717       for (j=0;j<N;j++)
718          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
719       for (j=0;j<N;j++)
720          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
721
722       interleave(X+C*eBands[i], C*N);
723
724
725       stereo_band_mix(m, X, bandE, 0, i, -1);
726       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
727       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
728    }
729    RESTORE_STACK;
730 }
731 #endif /* DISABLE_STEREO */
732
733 /* Decoding of the residual */
734 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
735 {
736    int i, j, remaining_bits, balance;
737    const celt_int16_t * restrict eBands = m->eBands;
738    celt_norm_t * restrict norm;
739    VARDECL(celt_norm_t, _norm);
740    int B;
741    SAVE_STACK;
742
743    B = shortBlocks ? m->nbShortMdcts : 1;
744    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
745    norm = _norm;
746
747    balance = 0;
748    for (i=0;i<m->nbEBands;i++)
749    {
750       int tell;
751       int N;
752       int q;
753       celt_word16_t n;
754       const celt_int16_t * const *BPbits;
755       
756       int curr_balance, curr_bits;
757
758       N = eBands[i+1]-eBands[i];
759       BPbits = m->bits;
760
761       tell = ec_dec_tell(dec, BITRES);
762       if (i != 0)
763          balance -= tell;
764       remaining_bits = (total_bits<<BITRES)-tell-1;
765       curr_balance = (m->nbEBands-i);
766       if (curr_balance > 3)
767          curr_balance = 3;
768       curr_balance = balance / curr_balance;
769       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
770       curr_bits = pulses2bits(BPbits[i], N, q);
771       remaining_bits -= curr_bits;
772       while (remaining_bits < 0 && q > 0)
773       {
774          remaining_bits += curr_bits;
775          q--;
776          curr_bits = pulses2bits(BPbits[i], N, q);
777          remaining_bits -= curr_bits;
778       }
779       balance += pulses[i] + tell;
780
781       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
782
783       if (q > 0)
784       {
785          int spread = fold ? B : 0;
786          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, dec);
787       } else {
788          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
789       }
790       for (j=eBands[i];j<eBands[i+1];j++)
791          norm[j] = MULT16_16_Q15(n,X[j]);
792    }
793    RESTORE_STACK;
794 }
795
796 #ifndef DISABLE_STEREO
797
798 void unquant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
799 {
800    int i, j, remaining_bits, balance;
801    const celt_int16_t * restrict eBands = m->eBands;
802    celt_norm_t * restrict norm;
803    VARDECL(celt_norm_t, _norm);
804    const int C = CHANNELS(m);
805    int B;
806    celt_word16_t mid, side;
807    SAVE_STACK;
808
809    B = shortBlocks ? m->nbShortMdcts : 1;
810    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
811    norm = _norm;
812
813    balance = 0;
814    for (i=0;i<m->nbEBands;i++)
815    {
816       int c;
817       int tell;
818       int q1, q2;
819       celt_word16_t n;
820       const celt_int16_t * const *BPbits;
821       int b, qb;
822       int N;
823       int curr_balance, curr_bits;
824       int imid, iside, itheta;
825       int mbits, sbits, delta;
826       int qalloc;
827       
828       BPbits = m->bits;
829
830       N = eBands[i+1]-eBands[i];
831       tell = ec_dec_tell(dec, BITRES);
832       if (i != 0)
833          balance -= tell;
834       remaining_bits = (total_bits<<BITRES)-tell-1;
835       curr_balance = (m->nbEBands-i);
836       if (curr_balance > 3)
837          curr_balance = 3;
838       curr_balance = balance / curr_balance;
839       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
840       if (b<0)
841          b = 0;
842
843       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
844       if (qb > (b>>BITRES)-1)
845          qb = (b>>BITRES)-1;
846       if (qb>14)
847          qb = 14;
848       if (qb<0)
849          qb = 0;
850       qalloc = log2_frac((1<<qb)+1,BITRES);
851       if (qb==0)
852       {
853          itheta=0;
854       } else {
855          int shift;
856          shift = 14-qb;
857          itheta = ec_dec_uint(dec, (1<<qb)+1);
858          itheta <<= shift;
859       }
860       if (itheta == 0)
861       {
862          imid = 32767;
863          iside = 0;
864          delta = -10000;
865       } else if (itheta == 16384)
866       {
867          imid = 0;
868          iside = 32767;
869          delta = 10000;
870       } else {
871          imid = bitexact_cos(itheta);
872          iside = bitexact_cos(16384-itheta);
873          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
874       }
875       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
876
877       if (N==2)
878       {
879          int c2;
880          int sign=1;
881          celt_norm_t v[2], w[2];
882          celt_norm_t *x2 = X+C*eBands[i];
883          mbits = b-qalloc;
884          sbits = 0;
885          if (itheta != 0 && itheta != 16384)
886             sbits = 1<<BITRES;
887          mbits -= sbits;
888          c = itheta > 8192 ? 1 : 0;
889          c2 = 1-c;
890
891          v[0] = x2[c];
892          v[1] = x2[c+C];
893          w[0] = x2[c2];
894          w[1] = x2[c2+C];
895          q1 = bits2pulses(m, BPbits[i], N, mbits);
896          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
897          remaining_bits -= curr_bits;
898          while (remaining_bits < 0 && q1 > 0)
899          {
900             remaining_bits += curr_bits;
901             q1--;
902             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
903             remaining_bits -= curr_bits;
904          }
905
906          if (q1 > 0)
907          {
908             int spread = fold ? B : 0;
909             alg_unquant(v, N, q1, spread, dec);
910          } else {
911             v[0] = QCONST16(1.f, 14);
912             v[1] = 0;
913          }
914          if (sbits)
915             sign = 2*ec_dec_bits(dec, 1)-1;
916          else
917             sign = 1;
918          w[0] = -sign*v[1];
919          w[1] = sign*v[0];
920          if (c==0)
921          {
922             x2[0] = v[0];
923             x2[1] = v[1];
924             x2[2] = w[0];
925             x2[3] = w[1];
926          } else {
927             x2[0] = w[0];
928             x2[1] = w[1];
929             x2[2] = v[0];
930             x2[3] = v[1];
931          }
932       } else {
933       mbits = (b-qalloc/2-delta)/2;
934       if (mbits > b-qalloc)
935          mbits = b-qalloc;
936       if (mbits<0)
937          mbits=0;
938       sbits = b-qalloc-mbits;
939       q1 = bits2pulses(m, BPbits[i], N, mbits);
940       q2 = bits2pulses(m, BPbits[i], N, sbits);
941       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
942       remaining_bits -= curr_bits;
943       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
944       {
945          remaining_bits += curr_bits;
946          if (q1>q2)
947          {
948             q1--;
949             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
950          } else {
951             q2--;
952             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
953          }
954          remaining_bits -= curr_bits;
955       }
956       
957
958
959       /* If pitch isn't available, use intra-frame prediction */
960       if (q1==0)
961       {
962          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
963          deinterleave(P+C*eBands[i], C*N);
964       }
965       deinterleave(X+C*eBands[i], C*N);
966       if (q1 > 0)
967       {
968          int spread = fold ? B : 0;
969          alg_unquant(X+C*eBands[i], N, q1, spread, dec);
970       } else
971          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
972             X[j] = P[j];
973       if (q2 > 0)
974       {
975          int spread = fold ? B : 0;
976          alg_unquant(X+C*eBands[i]+N, N, q2, spread, dec);
977       } else
978          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
979             X[j] = 0;
980       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
981       }
982       balance += pulses[i] + tell;
983
984 #ifdef FIXED_POINT
985       mid = imid;
986       side = iside;
987 #else
988       mid = (1./32768)*imid;
989       side = (1./32768)*iside;
990 #endif
991       for (c=0;c<C;c++)
992          for (j=0;j<N;j++)
993             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
994
995       for (j=0;j<N;j++)
996          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
997       for (j=0;j<N;j++)
998          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
999
1000       interleave(X+C*eBands[i], C*N);
1001
1002       stereo_band_mix(m, X, bandE, 0, i, -1);
1003       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1004       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1005    }
1006    RESTORE_STACK;
1007 }
1008
1009 #endif /* DISABLE_STEREO */