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