enabling denorm pitch only at low bit-rate
[opus.git] / libcelt / bands.c
1 /* (C) 2007-2008 Jean-Marc Valin, CSIRO
2    (C) 2008-2009 Gregory Maxwell */
3 /*
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.h>
37 #include "bands.h"
38 #include "modes.h"
39 #include "vq.h"
40 #include "cwrs.h"
41 #include "stack_alloc.h"
42 #include "os_support.h"
43 #include "mathops.h"
44 #include "rate.h"
45
46 const celt_word16_t sqrtC_1[2] = {QCONST16(1.f, 14), QCONST16(1.414214f, 14)};
47
48
49
50 #ifdef FIXED_POINT
51 /* Compute the amplitude (sqrt energy) in each of the bands */
52 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
53 {
54    int i, c, N;
55    const celt_int16_t *eBands = m->eBands;
56    const int C = CHANNELS(m);
57    N = FRAMESIZE(m);
58    for (c=0;c<C;c++)
59    {
60       for (i=0;i<m->nbEBands;i++)
61       {
62          int j;
63          celt_word32_t maxval=0;
64          celt_word32_t sum = 0;
65          
66          j=eBands[i]; do {
67             maxval = MAX32(maxval, X[j+c*N]);
68             maxval = MAX32(maxval, -X[j+c*N]);
69          } while (++j<eBands[i+1]);
70          
71          if (maxval > 0)
72          {
73             int shift = celt_ilog2(maxval)-10;
74             j=eBands[i]; do {
75                sum = MAC16_16(sum, EXTRACT16(VSHR32(X[j+c*N],shift)),
76                                    EXTRACT16(VSHR32(X[j+c*N],shift)));
77             } while (++j<eBands[i+1]);
78             /* We're adding one here to make damn sure we never end up with a pitch vector that's
79                larger than unity norm */
80             bank[i+c*m->nbEBands] = EPSILON+VSHR32(EXTEND32(celt_sqrt(sum)),-shift);
81          } else {
82             bank[i+c*m->nbEBands] = EPSILON;
83          }
84          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
85       }
86    }
87    /*printf ("\n");*/
88 }
89
90 /* Normalise each band such that the energy is one. */
91 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
92 {
93    int i, c, N;
94    const celt_int16_t *eBands = m->eBands;
95    const int C = CHANNELS(m);
96    N = FRAMESIZE(m);
97    for (c=0;c<C;c++)
98    {
99       i=0; do {
100          celt_word16_t g;
101          int j,shift;
102          celt_word16_t E;
103          shift = celt_zlog2(bank[i+c*m->nbEBands])-13;
104          E = VSHR32(bank[i+c*m->nbEBands], shift);
105          g = EXTRACT16(celt_rcp(SHL32(E,3)));
106          j=eBands[i]; do {
107             X[j*C+c] = MULT16_16_Q15(VSHR32(freq[j+c*N],shift-1),g);
108          } while (++j<eBands[i+1]);
109       } while (++i<m->nbEBands);
110    }
111 }
112
113 #else /* FIXED_POINT */
114 /* Compute the amplitude (sqrt energy) in each of the bands */
115 void compute_band_energies(const CELTMode *m, const celt_sig_t *X, celt_ener_t *bank)
116 {
117    int i, c, N;
118    const celt_int16_t *eBands = m->eBands;
119    const int C = CHANNELS(m);
120    N = FRAMESIZE(m);
121    for (c=0;c<C;c++)
122    {
123       for (i=0;i<m->nbEBands;i++)
124       {
125          int j;
126          celt_word32_t sum = 1e-10;
127          for (j=eBands[i];j<eBands[i+1];j++)
128             sum += X[j+c*N]*X[j+c*N];
129          bank[i+c*m->nbEBands] = sqrt(sum);
130          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
131       }
132    }
133    /*printf ("\n");*/
134 }
135
136 #ifdef EXP_PSY
137 void compute_noise_energies(const CELTMode *m, const celt_sig_t *X, const celt_word16_t *tonality, celt_ener_t *bank)
138 {
139    int i, c, N;
140    const celt_int16_t *eBands = m->eBands;
141    const int C = CHANNELS(m);
142    N = FRAMESIZE(m);
143    for (c=0;c<C;c++)
144    {
145       for (i=0;i<m->nbEBands;i++)
146       {
147          int j;
148          celt_word32_t sum = 1e-10;
149          for (j=eBands[i];j<eBands[i+1];j++)
150             sum += X[j*C+c]*X[j+c*N]*tonality[j];
151          bank[i+c*m->nbEBands] = sqrt(sum);
152          /*printf ("%f ", bank[i+c*m->nbEBands]);*/
153       }
154    }
155    /*printf ("\n");*/
156 }
157 #endif
158
159 /* Normalise each band such that the energy is one. */
160 void normalise_bands(const CELTMode *m, const celt_sig_t * restrict freq, celt_norm_t * restrict X, const celt_ener_t *bank)
161 {
162    int i, c, N;
163    const celt_int16_t *eBands = m->eBands;
164    const int C = CHANNELS(m);
165    N = FRAMESIZE(m);
166    for (c=0;c<C;c++)
167    {
168       for (i=0;i<m->nbEBands;i++)
169       {
170          int j;
171          celt_word16_t g = 1.f/(1e-10+bank[i+c*m->nbEBands]);
172          for (j=eBands[i];j<eBands[i+1];j++)
173             X[j*C+c] = freq[j+c*N]*g;
174       }
175    }
176 }
177
178 #endif /* FIXED_POINT */
179
180 #ifndef DISABLE_STEREO
181 void renormalise_bands(const CELTMode *m, celt_norm_t * restrict X)
182 {
183    int i, c;
184    const celt_int16_t *eBands = m->eBands;
185    const int C = CHANNELS(m);
186    for (c=0;c<C;c++)
187    {
188       i=0; do {
189          renormalise_vector(X+C*eBands[i]+c, QCONST16(0.70711f, 15), eBands[i+1]-eBands[i], C);
190       } while (++i<m->nbEBands);
191    }
192 }
193 #endif /* DISABLE_STEREO */
194
195 /* De-normalise the energy to produce the synthesis from the unit-energy bands */
196 void denormalise_bands(const CELTMode *m, const celt_norm_t * restrict X, celt_sig_t * restrict freq, const celt_ener_t *bank)
197 {
198    int i, c, N;
199    const celt_int16_t *eBands = m->eBands;
200    const int C = CHANNELS(m);
201    N = FRAMESIZE(m);
202    if (C>2)
203       celt_fatal("denormalise_bands() not implemented for >2 channels");
204    for (c=0;c<C;c++)
205    {
206       for (i=0;i<m->nbEBands;i++)
207       {
208          int j;
209          celt_word32_t g = SHR32(bank[i+c*m->nbEBands],1);
210          j=eBands[i]; do {
211             freq[j+c*N] = SHL32(MULT16_32_Q15(X[j*C+c], g),2);
212          } while (++j<eBands[i+1]);
213       }
214       for (i=eBands[m->nbEBands];i<eBands[m->nbEBands+1];i++)
215          freq[i+c*N] = 0;
216    }
217 }
218
219 int compute_new_pitch(const CELTMode *m, const celt_sig_t *X, const celt_sig_t *P, int norm_rate, int *gain_id)
220 {
221    int j ;
222    celt_word16_t g;
223    const int C = CHANNELS(m);
224    celt_word32_t Sxy=0, Sxx=0, Syy=0;
225    int len = 20*C;
226 #ifdef FIXED_POINT
227    int shift = 0;
228    celt_word32_t maxabs=0;
229    for (j=0;j<len;j++)
230    {
231       maxabs = MAX32(maxabs, ABS32(X[j]));
232       maxabs = MAX32(maxabs, ABS32(P[j]));
233    }
234    shift = celt_ilog2(maxabs)-12;
235    if (shift<0)
236       shift = 0;
237 #endif
238    for (j=0;j<len;j++)
239    {
240       celt_word16_t gg = Q15ONE-DIV32_16(MULT16_16(Q15ONE,j),len);
241       celt_word16_t Xj, Pj;
242       Xj = EXTRACT16(SHR32(X[j], shift));
243       Pj = MULT16_16_P15(gg,EXTRACT16(SHR32(P[j], shift)));
244       Sxy = MAC16_16(Sxy, Xj, Pj);
245       Sxx = MAC16_16(Sxx, Pj, Pj);
246       Syy = MAC16_16(Syy, Xj, Xj);
247    }
248 #ifdef FIXED_POINT
249    {
250       celt_word32_t num, den;
251       celt_word16_t fact;
252       fact = MULT16_16(QCONST16(.04, 14), norm_rate);
253       if (fact < QCONST16(1., 14))
254          fact = QCONST16(1., 14);
255       num = Sxy;
256       den = EPSILON+Sxx+MULT16_32_Q15(QCONST16(.03,15),Syy);
257       shift = celt_ilog2(Sxy)-16;
258       if (shift < 0)
259          shift = 0;
260       g = DIV32(SHL32(SHR32(num,shift),14),SHR32(den,shift));
261       if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
262          g = 0;
263       /* This MUST round down */
264       *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
265    }
266 #else
267    {
268       float fact = .04*norm_rate;
269       if (fact < 1)
270          fact = 1;
271       g = Sxy/(.1+Sxx+.03*Syy);
272       if (Sxy < .5*fact*celt_sqrt(1+Sxx*Syy))
273          g = 0;
274       *gain_id = floor(20*(g-.5));
275    }
276 #endif
277    if (*gain_id < 0)
278    {
279       *gain_id = 0;
280       return 0;
281    } else {
282       if (*gain_id > 15)
283          *gain_id = 15;
284       return 1;
285    }
286 }
287
288 void apply_new_pitch(const CELTMode *m, celt_sig_t *X, const celt_sig_t *P, int gain_id, int pred)
289 {
290    int j;
291    celt_word16_t gain;
292    const int C = CHANNELS(m);
293    int len = 20*C;
294    gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
295    if (pred)
296       gain = -gain;
297    for (j=0;j<len;j++)
298    {
299       celt_word16_t gg = SUB16(gain, DIV32_16(MULT16_16(gain,j),len));
300       X[j] += SHL(MULT16_32_Q15(gg,P[j]),1);
301    }
302 }
303
304 /* Compute the best gain for each "pitch band" */
305 int compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
306 {
307    int i;
308    int gain_sum = 0;
309    const celt_int16_t *pBands = m->pBands;
310    const int C = CHANNELS(m);
311
312    for (i=0;i<m->nbPBands;i++)
313    {
314       celt_word32_t Sxy=0, Sxx=0;
315       int j;
316       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
317       for (j=C*pBands[i];j<C*pBands[i+1];j++)
318       {
319          Sxy = MAC16_16(Sxy, X[j], P[j]);
320          Sxx = MAC16_16(Sxx, X[j], X[j]);
321       }
322       Sxy = SHR32(Sxy,2);
323       Sxx = SHR32(Sxx,2);
324       /* No negative gain allowed */
325       if (Sxy < 0)
326          Sxy = 0;
327       /* Not sure how that would happen, just making sure */
328       if (Sxy > Sxx)
329          Sxy = Sxx;
330       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
331          residual doesn't quantise well */
332       Sxy = MULT16_32_Q15(QCONST16(.99f, 15), Sxy);
333       /* gain = Sxy/Sxx */
334       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
335       if (gains[i]>QCONST16(.5,15))
336          gain_sum++;
337    }
338    return gain_sum > 5;
339 }
340
341 #ifndef DISABLE_STEREO
342
343 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int stereo_mode, int bandID, int dir)
344 {
345    int i = bandID;
346    const celt_int16_t *eBands = m->eBands;
347    const int C = CHANNELS(m);
348    int j;
349    celt_word16_t 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_t left, right;
357       celt_word16_t 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=eBands[i];j<eBands[i+1];j++)
368    {
369       celt_norm_t r, l;
370       l = X[j*C];
371       r = X[j*C+1];
372       X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
373       X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
374    }
375 }
376
377
378 void interleave(celt_norm_t *x, int N)
379 {
380    int i;
381    VARDECL(celt_norm_t, tmp);
382    SAVE_STACK;
383    ALLOC(tmp, N, celt_norm_t);
384    
385    for (i=0;i<N;i++)
386       tmp[i] = x[i];
387    for (i=0;i<N>>1;i++)
388    {
389       x[i<<1] = tmp[i];
390       x[(i<<1)+1] = tmp[i+(N>>1)];
391    }
392    RESTORE_STACK;
393 }
394
395 void deinterleave(celt_norm_t *x, int N)
396 {
397    int i;
398    VARDECL(celt_norm_t, tmp);
399    SAVE_STACK;
400    ALLOC(tmp, N, celt_norm_t);
401    
402    for (i=0;i<N;i++)
403       tmp[i] = x[i];
404    for (i=0;i<N>>1;i++)
405    {
406       x[i] = tmp[i<<1];
407       x[i+(N>>1)] = tmp[(i<<1)+1];
408    }
409    RESTORE_STACK;
410 }
411
412 #endif /* DISABLE_STEREO */
413
414 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
415 {
416    int i;
417    int NR=0;
418    celt_word32_t ratio = EPSILON;
419    const celt_int16_t * restrict eBands = m->eBands;
420    for (i=0;i<m->nbEBands;i++)
421    {
422       int j, N;
423       int max_i=0;
424       celt_word16_t max_val=EPSILON;
425       celt_word32_t floor_ener=EPSILON;
426       celt_norm_t * restrict x = X+eBands[i];
427       N = eBands[i+1]-eBands[i];
428       for (j=0;j<N;j++)
429       {
430          if (ABS16(x[j])>max_val)
431          {
432             max_val = ABS16(x[j]);
433             max_i = j;
434          }
435       }
436 #if 0
437       for (j=0;j<N;j++)
438       {
439          if (abs(j-max_i)>2)
440             floor_ener += x[j]*x[j];
441       }
442 #else
443       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
444       if (max_i < N-1)
445          floor_ener -= MULT16_16(x[max_i+1], x[max_i+1]);
446       if (max_i < N-2)
447          floor_ener -= MULT16_16(x[max_i+2], x[max_i+2]);
448       if (max_i > 0)
449          floor_ener -= MULT16_16(x[max_i-1], x[max_i-1]);
450       if (max_i > 1)
451          floor_ener -= MULT16_16(x[max_i-2], x[max_i-2]);
452       floor_ener = MAX32(floor_ener, EPSILON);
453 #endif
454       if (N>7 && eBands[i] >= m->pitchEnd)
455       {
456          celt_word16_t r;
457          celt_word16_t den = celt_sqrt(floor_ener);
458          den = MAX32(QCONST16(.02, 15), den);
459          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
460          ratio = ADD32(ratio, EXTEND32(r));
461          NR++;
462       }
463    }
464    if (NR>0)
465       ratio = DIV32_16(ratio, NR);
466    ratio = ADD32(HALF32(ratio), HALF32(*average));
467    if (!*last_decision)
468    {
469       *last_decision = (ratio < QCONST16(1.8,8));
470    } else {
471       *last_decision = (ratio < QCONST16(3.,8));
472    }
473    *average = EXTRACT16(ratio);
474    return *last_decision;
475 }
476
477 /* Quantisation of the residual */
478 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
479 {
480    int i, j, remaining_bits, balance;
481    const celt_int16_t * restrict eBands = m->eBands;
482    celt_norm_t * restrict norm;
483    VARDECL(celt_norm_t, _norm);   const celt_int16_t *pBands = m->pBands;
484    int pband=-1;
485    int B;
486    SAVE_STACK;
487
488    B = shortBlocks ? m->nbShortMdcts : 1;
489    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
490    norm = _norm;
491
492    balance = 0;
493    for (i=0;i<m->nbEBands;i++)
494    {
495       int tell;
496       int N;
497       int q;
498       celt_word16_t n;
499       const celt_int16_t * const *BPbits;
500       
501       int curr_balance, curr_bits;
502       
503       N = eBands[i+1]-eBands[i];
504       BPbits = m->bits;
505
506       tell = ec_enc_tell(enc, BITRES);
507       if (i != 0)
508          balance -= tell;
509       remaining_bits = (total_bits<<BITRES)-tell-1;
510       curr_balance = (m->nbEBands-i);
511       if (curr_balance > 3)
512          curr_balance = 3;
513       curr_balance = balance / curr_balance;
514       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
515       curr_bits = pulses2bits(BPbits[i], N, q);
516       remaining_bits -= curr_bits;
517       while (remaining_bits < 0 && q > 0)
518       {
519          remaining_bits += curr_bits;
520          q--;
521          curr_bits = pulses2bits(BPbits[i], N, q);
522          remaining_bits -= curr_bits;
523       }
524       balance += pulses[i] + tell;
525       
526       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
527
528       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
529       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
530       {
531          int enabled = 1;
532          pband++;
533          if (remaining_bits >= 1<<BITRES) {
534            enabled = pgains[pband] > QCONST16(.5,15);
535            ec_enc_bits(enc, enabled, 1);
536            balance += 1<<BITRES;
537          }
538          if (enabled)
539             pgains[pband] = QCONST16(.9,15);
540          else
541             pgains[pband] = 0;
542       }
543
544       /* If pitch isn't available, use intra-frame prediction */
545       if (q==0)
546       {
547          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], norm, P+eBands[i], eBands[i], B);
548       } else if (pitch_used && eBands[i] < m->pitchEnd) {
549          for (j=eBands[i];j<eBands[i+1];j++)
550             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
551       } else {
552          for (j=eBands[i];j<eBands[i+1];j++)
553             P[j] = 0;
554       }
555       
556       if (q > 0)
557       {
558          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
559          alg_quant(X+eBands[i], W+eBands[i], eBands[i+1]-eBands[i], q, spread, P+eBands[i], enc);
560       } else {
561          for (j=eBands[i];j<eBands[i+1];j++)
562             X[j] = P[j];
563       }
564       for (j=eBands[i];j<eBands[i+1];j++)
565          norm[j] = MULT16_16_Q15(n,X[j]);
566    }
567    RESTORE_STACK;
568 }
569
570 #ifndef DISABLE_STEREO
571
572 void quant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, celt_mask_t *W, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
573 {
574    int i, j, remaining_bits, balance;
575    const celt_int16_t * restrict eBands = m->eBands;
576    celt_norm_t * restrict norm;
577    VARDECL(celt_norm_t, _norm);
578    const int C = CHANNELS(m);
579    const celt_int16_t *pBands = m->pBands;
580    int pband=-1;
581    int B;
582    celt_word16_t mid, side;
583    SAVE_STACK;
584
585    B = shortBlocks ? m->nbShortMdcts : 1;
586    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
587    norm = _norm;
588
589    balance = 0;
590    for (i=0;i<m->nbEBands;i++)
591    {
592       int c;
593       int tell;
594       int q1, q2;
595       celt_word16_t n;
596       const celt_int16_t * const *BPbits;
597       int b, qb;
598       int N;
599       int curr_balance, curr_bits;
600       int imid, iside, itheta;
601       int mbits, sbits, delta;
602       int qalloc;
603       
604       BPbits = m->bits;
605
606       N = eBands[i+1]-eBands[i];
607       tell = ec_enc_tell(enc, BITRES);
608       if (i != 0)
609          balance -= tell;
610       remaining_bits = (total_bits<<BITRES)-tell-1;
611       curr_balance = (m->nbEBands-i);
612       if (curr_balance > 3)
613          curr_balance = 3;
614       curr_balance = balance / curr_balance;
615       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
616       if (b<0)
617          b = 0;
618
619       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
620       if (qb > (b>>BITRES)-1)
621          qb = (b>>BITRES)-1;
622       if (qb<0)
623          qb = 0;
624       if (qb>14)
625          qb = 14;
626
627       stereo_band_mix(m, X, bandE, qb==0, i, 1);
628
629       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
630       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
631 #ifdef FIXED_POINT
632       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
633 #else
634       itheta = floor(.5+16384*0.63662*atan2(side,mid));
635 #endif
636       qalloc = log2_frac((1<<qb)+1,BITRES);
637       if (qb==0)
638       {
639          itheta=0;
640       } else {
641          int shift;
642          shift = 14-qb;
643          itheta = (itheta+(1<<shift>>1))>>shift;
644          ec_enc_uint(enc, itheta, (1<<qb)+1);
645          itheta <<= shift;
646       }
647       if (itheta == 0)
648       {
649          imid = 32767;
650          iside = 0;
651          delta = -10000;
652       } else if (itheta == 16384)
653       {
654          imid = 0;
655          iside = 32767;
656          delta = 10000;
657       } else {
658          imid = bitexact_cos(itheta);
659          iside = bitexact_cos(16384-itheta);
660          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
661       }
662       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
663
664       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
665       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
666       {
667          int enabled = 1;
668          pband++;
669          if (remaining_bits >= 1<<BITRES) {
670             enabled = pgains[pband] > QCONST16(.5,15);
671             ec_enc_bits(enc, enabled, 1);
672             balance += 1<<BITRES;
673             remaining_bits -= 1<<BITRES;
674          }
675          if (enabled)
676             pgains[pband] = QCONST16(.9,15);
677          else
678             pgains[pband] = 0;
679       }
680
681       if (N==2)
682       {
683          int c2;
684          int sign=1;
685          celt_norm_t v[2], w[2];
686          celt_norm_t *x2 = X+C*eBands[i];
687          mbits = b-qalloc;
688          sbits = 0;
689          if (itheta != 0 && itheta != 16384)
690             sbits = 1<<BITRES;
691          mbits -= sbits;
692          c = itheta > 8192 ? 1 : 0;
693          c2 = 1-c;
694
695          if (eBands[i] >= m->pitchEnd && fold)
696          {
697          } else if (pitch_used && eBands[i] < m->pitchEnd) {
698             stereo_band_mix(m, P, bandE, qb==0, i, 1);
699             renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
700             renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
701             deinterleave(P+C*eBands[i], C*N);
702             for (j=C*eBands[i];j<C*eBands[i+1];j++)
703                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
704          } else {
705             for (j=C*eBands[i];j<C*eBands[i+1];j++)
706                P[j] = 0;
707          }
708          v[0] = x2[c];
709          v[1] = x2[c+C];
710          w[0] = x2[c2];
711          w[1] = x2[c2+C];
712          q1 = bits2pulses(m, BPbits[i], N, mbits);
713          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
714          remaining_bits -= curr_bits;
715          while (remaining_bits < 0 && q1 > 0)
716          {
717             remaining_bits += curr_bits;
718             q1--;
719             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
720             remaining_bits -= curr_bits;
721          }
722
723          if (q1 > 0)
724          {
725             int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
726             alg_quant(v, W+C*eBands[i], N, q1, spread, P+C*eBands[i]+c*N, enc);
727          } else {
728             v[0] = QCONST16(1.f, 14);
729             v[1] = 0;
730          }
731          if (sbits)
732          {
733             if (v[0]*w[1] - v[1]*w[0] > 0)
734                sign = 1;
735             else
736                sign = -1;
737             ec_enc_bits(enc, sign==1, 1);
738          } else {
739             sign = 1;
740          }
741          w[0] = -sign*v[1];
742          w[1] = sign*v[0];
743          if (c==0)
744          {
745             x2[0] = v[0];
746             x2[1] = v[1];
747             x2[2] = w[0];
748             x2[3] = w[1];
749          } else {
750             x2[0] = w[0];
751             x2[1] = w[1];
752             x2[2] = v[0];
753             x2[3] = v[1];
754          }
755       } else {
756          
757       mbits = (b-qalloc/2-delta)/2;
758       if (mbits > b-qalloc)
759          mbits = b-qalloc;
760       if (mbits<0)
761          mbits=0;
762       sbits = b-qalloc-mbits;
763       q1 = bits2pulses(m, BPbits[i], N, mbits);
764       q2 = bits2pulses(m, BPbits[i], N, sbits);
765       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
766       remaining_bits -= curr_bits;
767       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
768       {
769          remaining_bits += curr_bits;
770          if (q1>q2)
771          {
772             q1--;
773             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
774          } else {
775             q2--;
776             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
777          }
778          remaining_bits -= curr_bits;
779       }
780
781       /* If pitch isn't available, use intra-frame prediction */
782       if (q1+q2==0)
783       {
784          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
785          deinterleave(P+C*eBands[i], C*N);
786       } else if (pitch_used && eBands[i] < m->pitchEnd) {
787          stereo_band_mix(m, P, bandE, qb==0, i, 1);
788          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
789          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
790          deinterleave(P+C*eBands[i], C*N);
791          for (j=C*eBands[i];j<C*eBands[i+1];j++)
792             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
793       } else {
794          for (j=C*eBands[i];j<C*eBands[i+1];j++)
795             P[j] = 0;
796       }
797       deinterleave(X+C*eBands[i], C*N);
798       if (q1 > 0) {
799          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
800          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, spread, P+C*eBands[i], enc);
801       } else
802          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
803             X[j] = P[j];
804       if (q2 > 0) {
805          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
806          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, spread, P+C*eBands[i]+N, enc);
807       } else
808          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
809             X[j] = 0;
810       }
811       
812       balance += pulses[i] + tell;
813
814 #ifdef FIXED_POINT
815       mid = imid;
816       side = iside;
817 #else
818       mid = (1./32768)*imid;
819       side = (1./32768)*iside;
820 #endif
821       for (c=0;c<C;c++)
822          for (j=0;j<N;j++)
823             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
824
825       for (j=0;j<N;j++)
826          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
827       for (j=0;j<N;j++)
828          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
829
830       interleave(X+C*eBands[i], C*N);
831
832
833       stereo_band_mix(m, X, bandE, 0, i, -1);
834       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
835       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
836    }
837    RESTORE_STACK;
838 }
839 #endif /* DISABLE_STEREO */
840
841 /* Decoding of the residual */
842 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
843 {
844    int i, j, remaining_bits, balance;
845    const celt_int16_t * restrict eBands = m->eBands;
846    celt_norm_t * restrict norm;
847    VARDECL(celt_norm_t, _norm);
848    const celt_int16_t *pBands = m->pBands;
849    int pband=-1;
850    int B;
851    SAVE_STACK;
852
853    B = shortBlocks ? m->nbShortMdcts : 1;
854    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
855    norm = _norm;
856
857    balance = 0;
858    for (i=0;i<m->nbEBands;i++)
859    {
860       int tell;
861       int N;
862       int q;
863       celt_word16_t n;
864       const celt_int16_t * const *BPbits;
865       
866       int curr_balance, curr_bits;
867
868       N = eBands[i+1]-eBands[i];
869       BPbits = m->bits;
870
871       tell = ec_dec_tell(dec, BITRES);
872       if (i != 0)
873          balance -= tell;
874       remaining_bits = (total_bits<<BITRES)-tell-1;
875       curr_balance = (m->nbEBands-i);
876       if (curr_balance > 3)
877          curr_balance = 3;
878       curr_balance = balance / curr_balance;
879       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
880       curr_bits = pulses2bits(BPbits[i], N, q);
881       remaining_bits -= curr_bits;
882       while (remaining_bits < 0 && q > 0)
883       {
884          remaining_bits += curr_bits;
885          q--;
886          curr_bits = pulses2bits(BPbits[i], N, q);
887          remaining_bits -= curr_bits;
888       }
889       balance += pulses[i] + tell;
890
891       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
892
893       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
894       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
895       {
896          int enabled = 1;
897          pband++;
898          if (remaining_bits >= 1<<BITRES) {
899            enabled = ec_dec_bits(dec, 1);
900            balance += 1<<BITRES;
901          }
902          if (enabled)
903             pgains[pband] = QCONST16(.9,15);
904          else
905             pgains[pband] = 0;
906       }
907
908       /* If pitch isn't available, use intra-frame prediction */
909       if (q==0)
910       {
911          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], norm, P+eBands[i], eBands[i], B);
912       } else if (pitch_used && eBands[i] < m->pitchEnd) {
913          for (j=eBands[i];j<eBands[i+1];j++)
914             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
915       } else {
916          for (j=eBands[i];j<eBands[i+1];j++)
917             P[j] = 0;
918       }
919       
920       if (q > 0)
921       {
922          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
923          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, P+eBands[i], dec);
924       } else {
925          for (j=eBands[i];j<eBands[i+1];j++)
926             X[j] = P[j];
927       }
928       for (j=eBands[i];j<eBands[i+1];j++)
929          norm[j] = MULT16_16_Q15(n,X[j]);
930    }
931    RESTORE_STACK;
932 }
933
934 #ifndef DISABLE_STEREO
935
936 void unquant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, celt_norm_t *P, int pitch_used, celt_pgain_t *pgains, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
937 {
938    int i, j, remaining_bits, balance;
939    const celt_int16_t * restrict eBands = m->eBands;
940    celt_norm_t * restrict norm;
941    VARDECL(celt_norm_t, _norm);
942    const int C = CHANNELS(m);
943    const celt_int16_t *pBands = m->pBands;
944    int pband=-1;
945    int B;
946    celt_word16_t mid, side;
947    SAVE_STACK;
948
949    B = shortBlocks ? m->nbShortMdcts : 1;
950    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
951    norm = _norm;
952
953    balance = 0;
954    for (i=0;i<m->nbEBands;i++)
955    {
956       int c;
957       int tell;
958       int q1, q2;
959       celt_word16_t n;
960       const celt_int16_t * const *BPbits;
961       int b, qb;
962       int N;
963       int curr_balance, curr_bits;
964       int imid, iside, itheta;
965       int mbits, sbits, delta;
966       int qalloc;
967       
968       BPbits = m->bits;
969
970       N = eBands[i+1]-eBands[i];
971       tell = ec_dec_tell(dec, BITRES);
972       if (i != 0)
973          balance -= tell;
974       remaining_bits = (total_bits<<BITRES)-tell-1;
975       curr_balance = (m->nbEBands-i);
976       if (curr_balance > 3)
977          curr_balance = 3;
978       curr_balance = balance / curr_balance;
979       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
980       if (b<0)
981          b = 0;
982
983       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
984       if (qb > (b>>BITRES)-1)
985          qb = (b>>BITRES)-1;
986       if (qb>14)
987          qb = 14;
988       if (qb<0)
989          qb = 0;
990       qalloc = log2_frac((1<<qb)+1,BITRES);
991       if (qb==0)
992       {
993          itheta=0;
994       } else {
995          int shift;
996          shift = 14-qb;
997          itheta = ec_dec_uint(dec, (1<<qb)+1);
998          itheta <<= shift;
999       }
1000       if (itheta == 0)
1001       {
1002          imid = 32767;
1003          iside = 0;
1004          delta = -10000;
1005       } else if (itheta == 16384)
1006       {
1007          imid = 0;
1008          iside = 32767;
1009          delta = 10000;
1010       } else {
1011          imid = bitexact_cos(itheta);
1012          iside = bitexact_cos(16384-itheta);
1013          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
1014       }
1015       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
1016
1017       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
1018       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
1019       {
1020          int enabled = 1;
1021          pband++;
1022          if (remaining_bits >= 1<<BITRES) {
1023             enabled = ec_dec_bits(dec, 1);
1024             balance += 1<<BITRES;
1025             remaining_bits -= 1<<BITRES;
1026          }
1027          if (enabled)
1028             pgains[pband] = QCONST16(.9,15);
1029          else
1030             pgains[pband] = 0;
1031       }
1032
1033       if (N==2)
1034       {
1035          int c2;
1036          int sign=1;
1037          celt_norm_t v[2], w[2];
1038          celt_norm_t *x2 = X+C*eBands[i];
1039          mbits = b-qalloc;
1040          sbits = 0;
1041          if (itheta != 0 && itheta != 16384)
1042             sbits = 1<<BITRES;
1043          mbits -= sbits;
1044          c = itheta > 8192 ? 1 : 0;
1045          c2 = 1-c;
1046
1047          if (eBands[i] >= m->pitchEnd && fold)
1048          {
1049          } else if (pitch_used && eBands[i] < m->pitchEnd) {
1050             stereo_band_mix(m, P, bandE, qb==0, i, 1);
1051             renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1052             renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1053             deinterleave(P+C*eBands[i], C*N);
1054             for (j=C*eBands[i];j<C*eBands[i+1];j++)
1055                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1056          } else {
1057             for (j=C*eBands[i];j<C*eBands[i+1];j++)
1058                P[j] = 0;
1059          }
1060          v[0] = x2[c];
1061          v[1] = x2[c+C];
1062          w[0] = x2[c2];
1063          w[1] = x2[c2+C];
1064          q1 = bits2pulses(m, BPbits[i], N, mbits);
1065          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
1066          remaining_bits -= curr_bits;
1067          while (remaining_bits < 0 && q1 > 0)
1068          {
1069             remaining_bits += curr_bits;
1070             q1--;
1071             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
1072             remaining_bits -= curr_bits;
1073          }
1074
1075          if (q1 > 0)
1076          {
1077             int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
1078             alg_unquant(v, N, q1, spread, P+C*eBands[i]+c*N, dec);
1079          } else {
1080             v[0] = QCONST16(1.f, 14);
1081             v[1] = 0;
1082          }
1083          if (sbits)
1084             sign = 2*ec_dec_bits(dec, 1)-1;
1085          else
1086             sign = 1;
1087          w[0] = -sign*v[1];
1088          w[1] = sign*v[0];
1089          if (c==0)
1090          {
1091             x2[0] = v[0];
1092             x2[1] = v[1];
1093             x2[2] = w[0];
1094             x2[3] = w[1];
1095          } else {
1096             x2[0] = w[0];
1097             x2[1] = w[1];
1098             x2[2] = v[0];
1099             x2[3] = v[1];
1100          }
1101       } else {
1102       mbits = (b-qalloc/2-delta)/2;
1103       if (mbits > b-qalloc)
1104          mbits = b-qalloc;
1105       if (mbits<0)
1106          mbits=0;
1107       sbits = b-qalloc-mbits;
1108       q1 = bits2pulses(m, BPbits[i], N, mbits);
1109       q2 = bits2pulses(m, BPbits[i], N, sbits);
1110       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1111       remaining_bits -= curr_bits;
1112       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
1113       {
1114          remaining_bits += curr_bits;
1115          if (q1>q2)
1116          {
1117             q1--;
1118             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1119          } else {
1120             q2--;
1121             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1122          }
1123          remaining_bits -= curr_bits;
1124       }
1125       
1126
1127
1128       /* If pitch isn't available, use intra-frame prediction */
1129       if (q1+q2==0)
1130       {
1131          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
1132          deinterleave(P+C*eBands[i], C*N);
1133       } else if (pitch_used && eBands[i] < m->pitchEnd) {
1134          stereo_band_mix(m, P, bandE, qb==0, i, 1);
1135          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1136          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1137          deinterleave(P+C*eBands[i], C*N);
1138          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1139             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1140       } else {
1141          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1142             P[j] = 0;
1143       }
1144       deinterleave(X+C*eBands[i], C*N);
1145       if (q1 > 0)
1146       {
1147          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
1148          alg_unquant(X+C*eBands[i], N, q1, spread, P+C*eBands[i], dec);
1149       } else
1150          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
1151             X[j] = P[j];
1152       if (q2 > 0)
1153       {
1154          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
1155          alg_unquant(X+C*eBands[i]+N, N, q2, spread, P+C*eBands[i]+N, dec);
1156       } else
1157          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
1158             X[j] = 0;
1159       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
1160       }
1161       balance += pulses[i] + tell;
1162
1163 #ifdef FIXED_POINT
1164       mid = imid;
1165       side = iside;
1166 #else
1167       mid = (1./32768)*imid;
1168       side = (1./32768)*iside;
1169 #endif
1170       for (c=0;c<C;c++)
1171          for (j=0;j<N;j++)
1172             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
1173
1174       for (j=0;j<N;j++)
1175          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
1176       for (j=0;j<N;j++)
1177          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
1178
1179       interleave(X+C*eBands[i], C*N);
1180
1181       stereo_band_mix(m, X, bandE, 0, i, -1);
1182       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1183       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1184    }
1185    RESTORE_STACK;
1186 }
1187
1188 #endif /* DISABLE_STEREO */