Changing the encoder API to add the frame size
[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, int M)
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=M*eBands[i]; do {
66             maxval = MAX32(maxval, X[j+c*N]);
67             maxval = MAX32(maxval, -X[j+c*N]);
68          } while (++j<M*eBands[i+1]);
69          
70          if (maxval > 0)
71          {
72             int shift = celt_ilog2(maxval)-10;
73             j=M*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<M*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, int M)
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=M*eBands[i]; do {
106             X[j+c*N] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
107          } while (++j<M*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, int M)
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=M*eBands[i];j<M*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, int M)
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=M*eBands[i];j<M*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, int M)
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=M*eBands[i];j<M*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, int M)
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+M*eBands[i]+c*M*eBands[m->nbEBands+1], Q15ONE, M*eBands[i+1]-M*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, int M)
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=M*eBands[i];
212          end = M*eBands[i+1];
213          do {
214             *f++ = SHL32(MULT16_32_Q15(*x, g),2);
215             x++;
216          } while (++j<end);
217       }
218       for (i=M*eBands[m->nbEBands];i<M*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, int M)
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<M*eBands[i+1]-M*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, int M)
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+M*eBands[i]+c*N0;
399       N = M*eBands[i+1]-M*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, int LM)
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    int M;
459    SAVE_STACK;
460
461    M = 1<<LM;
462    B = shortBlocks ? M : 1;
463    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
464    norm = _norm;
465
466    balance = 0;
467    for (i=start;i<m->nbEBands;i++)
468    {
469       int tell;
470       int N;
471       int q;
472       const celt_int16 * const *BPbits;
473       
474       int curr_balance, curr_bits;
475       
476       N = M*eBands[i+1]-M*eBands[i];
477       BPbits = m->bits[LM];
478
479       if (encode)
480          tell = ec_enc_tell(enc_dec, BITRES);
481       else
482          tell = ec_dec_tell(enc_dec, BITRES);
483       if (i != start)
484          balance -= tell;
485       remaining_bits = (total_bits<<BITRES)-tell-1;
486       curr_balance = (m->nbEBands-i);
487       if (curr_balance > 3)
488          curr_balance = 3;
489       curr_balance = balance / curr_balance;
490       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
491       curr_bits = pulses2bits(BPbits[i], N, q);
492       remaining_bits -= curr_bits;
493       while (remaining_bits < 0 && q > 0)
494       {
495          remaining_bits += curr_bits;
496          q--;
497          curr_bits = pulses2bits(BPbits[i], N, q);
498          remaining_bits -= curr_bits;
499       }
500       balance += pulses[i] + tell;
501       
502
503       if (q > 0)
504       {
505          int spread = fold ? B : 0;
506          if (encode)
507             alg_quant(X+M*eBands[i], N, q, spread, resynth, enc_dec);
508          else
509             alg_unquant(X+M*eBands[i], N, q, spread, enc_dec);
510       } else {
511          if (resynth)
512             intra_fold(m, start, N, norm, X+M*eBands[i], M*eBands[i], B, M);
513       }
514       if (resynth)
515       {
516          celt_word16 n;
517          n = celt_sqrt(SHL32(EXTEND32(N),22));
518          for (j=M*eBands[i];j<M*eBands[i+1];j++)
519             norm[j] = MULT16_16_Q15(n,X[j]);
520       }
521    }
522    RESTORE_STACK;
523 }
524
525 #ifndef DISABLE_STEREO
526
527 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, int LM)
528 {
529    int i, j, remaining_bits, balance;
530    const celt_int16 * restrict eBands = m->eBands;
531    celt_norm * restrict norm;
532    VARDECL(celt_norm, _norm);
533    int B;
534    celt_word16 mid, side;
535    int M;
536    SAVE_STACK;
537
538    M = 1<<LM;
539    B = shortBlocks ? M : 1;
540    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
541    norm = _norm;
542
543    balance = 0;
544    for (i=start;i<m->nbEBands;i++)
545    {
546       int tell;
547       int q1, q2;
548       const celt_int16 * const *BPbits;
549       int b, qb;
550       int N;
551       int curr_balance, curr_bits;
552       int imid, iside, itheta;
553       int mbits, sbits, delta;
554       int qalloc;
555       celt_norm * restrict X, * restrict Y;
556       
557       X = _X+M*eBands[i];
558       Y = X+M*eBands[m->nbEBands+1];
559       BPbits = m->bits[LM];
560
561       N = M*eBands[i+1]-M*eBands[i];
562       tell = ec_enc_tell(enc, BITRES);
563       if (i != start)
564          balance -= tell;
565       remaining_bits = (total_bits<<BITRES)-tell-1;
566       curr_balance = (m->nbEBands-i);
567       if (curr_balance > 3)
568          curr_balance = 3;
569       curr_balance = balance / curr_balance;
570       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
571       if (b<0)
572          b = 0;
573
574       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
575       if (qb > (b>>BITRES)-1)
576          qb = (b>>BITRES)-1;
577       if (qb<0)
578          qb = 0;
579       if (qb>14)
580          qb = 14;
581
582       stereo_band_mix(m, X, Y, bandE, qb==0, i, 1, M);
583
584       mid = renormalise_vector(X, Q15ONE, N, 1);
585       side = renormalise_vector(Y, Q15ONE, N, 1);
586 #ifdef FIXED_POINT
587       itheta = MULT16_16_Q15(QCONST16(0.63662f,15),celt_atan2p(side, mid));
588 #else
589       itheta = floor(.5f+16384*0.63662f*atan2(side,mid));
590 #endif
591       qalloc = log2_frac((1<<qb)+1,BITRES);
592       if (qb==0)
593       {
594          itheta=0;
595       } else {
596          int shift;
597          shift = 14-qb;
598          itheta = (itheta+(1<<shift>>1))>>shift;
599          ec_enc_uint(enc, itheta, (1<<qb)+1);
600          itheta <<= shift;
601       }
602       if (itheta == 0)
603       {
604          imid = 32767;
605          iside = 0;
606          delta = -10000;
607       } else if (itheta == 16384)
608       {
609          imid = 0;
610          iside = 32767;
611          delta = 10000;
612       } else {
613          imid = bitexact_cos(itheta);
614          iside = bitexact_cos(16384-itheta);
615          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
616       }
617 #if 1
618       if (N==2)
619       {
620          int c, c2;
621          int sign=1;
622          celt_norm v[2], w[2];
623          celt_norm *x2, *y2;
624          mbits = b-qalloc;
625          sbits = 0;
626          if (itheta != 0 && itheta != 16384)
627             sbits = 1<<BITRES;
628          mbits -= sbits;
629          c = itheta > 8192 ? 1 : 0;
630          c2 = 1-c;
631
632          x2 = X;
633          y2 = Y;
634          if (c==0)
635          {
636             v[0] = x2[0];
637             v[1] = x2[1];
638             w[0] = y2[0];
639             w[1] = y2[1];
640          } else {
641             v[0] = y2[0];
642             v[1] = y2[1];
643             w[0] = x2[0];
644             w[1] = x2[1];
645          }
646          q1 = bits2pulses(m, BPbits[i], N, mbits);
647          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
648          remaining_bits -= curr_bits;
649          while (remaining_bits < 0 && q1 > 0)
650          {
651             remaining_bits += curr_bits;
652             q1--;
653             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
654             remaining_bits -= curr_bits;
655          }
656
657          if (q1 > 0)
658          {
659             int spread = fold ? B : 0;
660             alg_quant(v, N, q1, spread, resynth, enc);
661          } else {
662             v[0] = QCONST16(1.f, 14);
663             v[1] = 0;
664          }
665          if (sbits)
666          {
667             if (v[0]*w[1] - v[1]*w[0] > 0)
668                sign = 1;
669             else
670                sign = -1;
671             ec_enc_bits(enc, sign==1, 1);
672          } else {
673             sign = 1;
674          }
675          w[0] = -sign*v[1];
676          w[1] = sign*v[0];
677          if (c==0)
678          {
679             x2[0] = v[0];
680             x2[1] = v[1];
681             y2[0] = w[0];
682             y2[1] = w[1];
683          } else {
684             x2[0] = w[0];
685             x2[1] = w[1];
686             y2[0] = v[0];
687             y2[1] = v[1];
688          }
689       } else 
690 #endif
691       {
692          
693          mbits = (b-qalloc/2-delta)/2;
694          if (mbits > b-qalloc)
695             mbits = b-qalloc;
696          if (mbits<0)
697             mbits=0;
698          sbits = b-qalloc-mbits;
699          q1 = bits2pulses(m, BPbits[i], N, mbits);
700          q2 = bits2pulses(m, BPbits[i], N, sbits);
701          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
702          remaining_bits -= curr_bits;
703          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
704          {
705             remaining_bits += curr_bits;
706             if (q1>q2)
707             {
708                q1--;
709                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
710             } else {
711                q2--;
712                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
713             }
714             remaining_bits -= curr_bits;
715          }
716
717          if (q1 > 0) {
718             int spread = fold ? B : 0;
719             alg_quant(X, N, q1, spread, resynth, enc);
720          } else {
721             if (resynth)
722                intra_fold(m, start, N, norm, X, M*eBands[i], B, M);
723          }
724          if (q2 > 0) {
725             int spread = fold ? B : 0;
726             alg_quant(Y, N, q2, spread, resynth, enc);
727          } else
728             for (j=0;j<N;j++)
729                Y[j] = 0;
730       }
731       
732       balance += pulses[i] + tell;
733
734       if (resynth)
735       {
736          celt_word16 n;
737 #ifdef FIXED_POINT
738          mid = imid;
739          side = iside;
740 #else
741          mid = (1.f/32768)*imid;
742          side = (1.f/32768)*iside;
743 #endif
744          n = celt_sqrt(SHL32(EXTEND32(N),22));
745          for (j=0;j<N;j++)
746             norm[M*eBands[i]+j] = MULT16_16_Q15(n,X[j]);
747
748          for (j=0;j<N;j++)
749             X[j] = MULT16_16_Q15(X[j], mid);
750          for (j=0;j<N;j++)
751             Y[j] = MULT16_16_Q15(Y[j], side);
752
753          stereo_band_mix(m, X, Y, bandE, 0, i, -1, M);
754          renormalise_vector(X, Q15ONE, N, 1);
755          renormalise_vector(Y, Q15ONE, N, 1);
756       }
757    }
758    RESTORE_STACK;
759 }
760 #endif /* DISABLE_STEREO */
761
762
763 #ifndef DISABLE_STEREO
764
765 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, int LM)
766 {
767    int i, j, remaining_bits, balance;
768    const celt_int16 * restrict eBands = m->eBands;
769    celt_norm * restrict norm;
770    VARDECL(celt_norm, _norm);
771    int B;
772    celt_word16 mid, side;
773    int M;
774    SAVE_STACK;
775
776    M = 1<<LM;
777    B = shortBlocks ? M : 1;
778    ALLOC(_norm, M*eBands[m->nbEBands+1], celt_norm);
779    norm = _norm;
780
781    balance = 0;
782    for (i=start;i<m->nbEBands;i++)
783    {
784       int tell;
785       int q1, q2;
786       celt_word16 n;
787       const celt_int16 * const *BPbits;
788       int b, qb;
789       int N;
790       int curr_balance, curr_bits;
791       int imid, iside, itheta;
792       int mbits, sbits, delta;
793       int qalloc;
794       celt_norm * restrict X, * restrict Y;
795       
796       X = _X+M*eBands[i];
797       Y = X+M*eBands[m->nbEBands+1];
798       BPbits = m->bits[LM];
799
800       N = M*eBands[i+1]-M*eBands[i];
801       tell = ec_dec_tell(dec, BITRES);
802       if (i != start)
803          balance -= tell;
804       remaining_bits = (total_bits<<BITRES)-tell-1;
805       curr_balance = (m->nbEBands-i);
806       if (curr_balance > 3)
807          curr_balance = 3;
808       curr_balance = balance / curr_balance;
809       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
810       if (b<0)
811          b = 0;
812
813       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
814       if (qb > (b>>BITRES)-1)
815          qb = (b>>BITRES)-1;
816       if (qb>14)
817          qb = 14;
818       if (qb<0)
819          qb = 0;
820       qalloc = log2_frac((1<<qb)+1,BITRES);
821       if (qb==0)
822       {
823          itheta=0;
824       } else {
825          int shift;
826          shift = 14-qb;
827          itheta = ec_dec_uint(dec, (1<<qb)+1);
828          itheta <<= shift;
829       }
830       if (itheta == 0)
831       {
832          imid = 32767;
833          iside = 0;
834          delta = -10000;
835       } else if (itheta == 16384)
836       {
837          imid = 0;
838          iside = 32767;
839          delta = 10000;
840       } else {
841          imid = bitexact_cos(itheta);
842          iside = bitexact_cos(16384-itheta);
843          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
844       }
845       n = celt_sqrt(SHL32(EXTEND32(N),22));
846
847 #if 1
848       if (N==2)
849       {
850          int c, c2;
851          int sign=1;
852          celt_norm v[2], w[2];
853          celt_norm *x2, *y2;
854          mbits = b-qalloc;
855          sbits = 0;
856          if (itheta != 0 && itheta != 16384)
857             sbits = 1<<BITRES;
858          mbits -= sbits;
859          c = itheta > 8192 ? 1 : 0;
860          c2 = 1-c;
861
862          x2 = X;
863          y2 = Y;
864          v[0] = x2[c];
865          v[1] = y2[c];
866          w[0] = x2[c2];
867          w[1] = y2[c2];
868          q1 = bits2pulses(m, BPbits[i], N, mbits);
869          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
870          remaining_bits -= curr_bits;
871          while (remaining_bits < 0 && q1 > 0)
872          {
873             remaining_bits += curr_bits;
874             q1--;
875             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
876             remaining_bits -= curr_bits;
877          }
878
879          if (q1 > 0)
880          {
881             int spread = fold ? B : 0;
882             alg_unquant(v, N, q1, spread, dec);
883          } else {
884             v[0] = QCONST16(1.f, 14);
885             v[1] = 0;
886          }
887          if (sbits)
888             sign = 2*ec_dec_bits(dec, 1)-1;
889          else
890             sign = 1;
891          w[0] = -sign*v[1];
892          w[1] = sign*v[0];
893          if (c==0)
894          {
895             x2[0] = v[0];
896             x2[1] = v[1];
897             y2[0] = w[0];
898             y2[1] = w[1];
899          } else {
900             x2[0] = w[0];
901             x2[1] = w[1];
902             y2[0] = v[0];
903             y2[1] = v[1];
904          }
905       } else
906 #endif
907       {
908          mbits = (b-qalloc/2-delta)/2;
909          if (mbits > b-qalloc)
910             mbits = b-qalloc;
911          if (mbits<0)
912             mbits=0;
913          sbits = b-qalloc-mbits;
914          q1 = bits2pulses(m, BPbits[i], N, mbits);
915          q2 = bits2pulses(m, BPbits[i], N, sbits);
916          curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
917          remaining_bits -= curr_bits;
918          while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
919          {
920             remaining_bits += curr_bits;
921             if (q1>q2)
922             {
923                q1--;
924                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
925             } else {
926                q2--;
927                curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
928             }
929             remaining_bits -= curr_bits;
930          }
931          
932          if (q1 > 0)
933          {
934             int spread = fold ? B : 0;
935             alg_unquant(X, N, q1, spread, dec);
936          } else
937             intra_fold(m, start, N, norm, X, M*eBands[i], B, M);
938          if (q2 > 0)
939          {
940             int spread = fold ? B : 0;
941             alg_unquant(Y, N, q2, spread, dec);
942          } else
943             for (j=0;j<N;j++)
944                Y[j] = 0;
945             /*orthogonalize(X+C*M*eBands[i], X+C*M*eBands[i]+N, N);*/
946       }
947       balance += pulses[i] + tell;
948       
949 #ifdef FIXED_POINT
950       mid = imid;
951       side = iside;
952 #else
953       mid = (1.f/32768)*imid;
954       side = (1.f/32768)*iside;
955 #endif
956       for (j=0;j<N;j++)
957          norm[M*eBands[i]+j] = MULT16_16_Q15(n,X[j]);
958
959       for (j=0;j<N;j++)
960          X[j] = MULT16_16_Q15(X[j], mid);
961       for (j=0;j<N;j++)
962          Y[j] = MULT16_16_Q15(Y[j], side);
963
964       stereo_band_mix(m, X, Y, bandE, 0, i, -1, M);
965       renormalise_vector(X, Q15ONE, N, 1);
966       renormalise_vector(Y, Q15ONE, N, 1);
967    }
968    RESTORE_STACK;
969 }
970
971 #endif /* DISABLE_STEREO */