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