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