denorm pitch works in fixed-point (though there's still some floats left)
[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 *gain_id)
220 {
221    int j ;
222    float g;
223    const int C = CHANNELS(m);
224    float Sxy=0, Sxx=0, Syy=0;
225    int len = 20*C;
226    
227    for (j=0;j<len;j++)
228    {
229       float gg = 1-1.*j/len;
230 #ifdef FIXED_POINT
231       Sxy += X[j] * gg*P[j];
232       Sxx += gg*P[j]* gg*P[j];
233       Syy += X[j] *1.*X[j];
234 #else
235       Sxy = MAC16_16(Sxy, X[j], gg*P[j]);
236       Sxx = MAC16_16(Sxx, gg*P[j], gg*P[j]);
237       Syy = MAC16_16(Syy, X[j], X[j]);
238 #endif
239    }
240    g = QCONST16(1.,14)*Sxy/(.1+Sxx+.03*Syy);
241    if (Sxy/sqrt(.1+Sxx*Syy) < .5)
242       g = 0;
243 #ifdef FIXED_POINT
244    /* This MUST round down */
245    *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
246 #else
247    *gain_id = floor(20*(g-.5));
248 #endif
249    if (*gain_id < 0)
250    {
251       *gain_id = 0;
252       return 0;
253    } else {
254       if (*gain_id > 15)
255          *gain_id = 15;
256       return 1;
257    }
258 }
259
260 void apply_new_pitch(const CELTMode *m, celt_sig_t *X, const celt_sig_t *P, int gain_id, int pred)
261 {
262    int j;
263    celt_word16_t gain;
264    const int C = CHANNELS(m);
265    int len = 20*C;
266    gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
267    if (pred)
268       gain = -gain;
269    for (j=0;j<len;j++)
270    {
271       celt_word16_t gg = SUB16(gain, DIV32_16(MULT16_16(gain,j),len));
272       X[j] += SHL(MULT16_32_Q15(gg,P[j]),1);
273    }
274 }
275
276 /* Compute the best gain for each "pitch band" */
277 int compute_pitch_gain(const CELTMode *m, const celt_norm_t *X, const celt_norm_t *P, celt_pgain_t *gains)
278 {
279    int i;
280    int gain_sum = 0;
281    const celt_int16_t *pBands = m->pBands;
282    const int C = CHANNELS(m);
283
284    for (i=0;i<m->nbPBands;i++)
285    {
286       celt_word32_t Sxy=0, Sxx=0;
287       int j;
288       /* We know we're not going to overflow because Sxx can't be more than 1 (Q28) */
289       for (j=C*pBands[i];j<C*pBands[i+1];j++)
290       {
291          Sxy = MAC16_16(Sxy, X[j], P[j]);
292          Sxx = MAC16_16(Sxx, X[j], X[j]);
293       }
294       Sxy = SHR32(Sxy,2);
295       Sxx = SHR32(Sxx,2);
296       /* No negative gain allowed */
297       if (Sxy < 0)
298          Sxy = 0;
299       /* Not sure how that would happen, just making sure */
300       if (Sxy > Sxx)
301          Sxy = Sxx;
302       /* We need to be a bit conservative (multiply gain by 0.9), otherwise the
303          residual doesn't quantise well */
304       Sxy = MULT16_32_Q15(QCONST16(.99f, 15), Sxy);
305       /* gain = Sxy/Sxx */
306       gains[i] = EXTRACT16(celt_div(Sxy,ADD32(SHR32(Sxx, PGAIN_SHIFT),EPSILON)));
307       if (gains[i]>QCONST16(.5,15))
308          gain_sum++;
309    }
310    return gain_sum > 5;
311 }
312
313 #ifndef DISABLE_STEREO
314
315 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int stereo_mode, int bandID, int dir)
316 {
317    int i = bandID;
318    const celt_int16_t *eBands = m->eBands;
319    const int C = CHANNELS(m);
320    int j;
321    celt_word16_t a1, a2;
322    if (stereo_mode==0)
323    {
324       /* Do mid-side when not doing intensity stereo */
325       a1 = QCONST16(.70711f,14);
326       a2 = dir*QCONST16(.70711f,14);
327    } else {
328       celt_word16_t left, right;
329       celt_word16_t norm;
330 #ifdef FIXED_POINT
331       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
332 #endif
333       left = VSHR32(bank[i],shift);
334       right = VSHR32(bank[i+m->nbEBands],shift);
335       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
336       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
337       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
338    }
339    for (j=eBands[i];j<eBands[i+1];j++)
340    {
341       celt_norm_t r, l;
342       l = X[j*C];
343       r = X[j*C+1];
344       X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
345       X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
346    }
347 }
348
349
350 void interleave(celt_norm_t *x, int N)
351 {
352    int i;
353    VARDECL(celt_norm_t, tmp);
354    SAVE_STACK;
355    ALLOC(tmp, N, celt_norm_t);
356    
357    for (i=0;i<N;i++)
358       tmp[i] = x[i];
359    for (i=0;i<N>>1;i++)
360    {
361       x[i<<1] = tmp[i];
362       x[(i<<1)+1] = tmp[i+(N>>1)];
363    }
364    RESTORE_STACK;
365 }
366
367 void deinterleave(celt_norm_t *x, int N)
368 {
369    int i;
370    VARDECL(celt_norm_t, tmp);
371    SAVE_STACK;
372    ALLOC(tmp, N, celt_norm_t);
373    
374    for (i=0;i<N;i++)
375       tmp[i] = x[i];
376    for (i=0;i<N>>1;i++)
377    {
378       x[i] = tmp[i<<1];
379       x[i+(N>>1)] = tmp[(i<<1)+1];
380    }
381    RESTORE_STACK;
382 }
383
384 #endif /* DISABLE_STEREO */
385
386 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
387 {
388    int i;
389    int NR=0;
390    celt_word32_t ratio = EPSILON;
391    const celt_int16_t * restrict eBands = m->eBands;
392    for (i=0;i<m->nbEBands;i++)
393    {
394       int j, N;
395       int max_i=0;
396       celt_word16_t max_val=EPSILON;
397       celt_word32_t floor_ener=EPSILON;
398       celt_norm_t * restrict x = X+eBands[i];
399       N = eBands[i+1]-eBands[i];
400       for (j=0;j<N;j++)
401       {
402          if (ABS16(x[j])>max_val)
403          {
404             max_val = ABS16(x[j]);
405             max_i = j;
406          }
407       }
408 #if 0
409       for (j=0;j<N;j++)
410       {
411          if (abs(j-max_i)>2)
412             floor_ener += x[j]*x[j];
413       }
414 #else
415       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
416       if (max_i < N-1)
417          floor_ener -= MULT16_16(x[max_i+1], x[max_i+1]);
418       if (max_i < N-2)
419          floor_ener -= MULT16_16(x[max_i+2], x[max_i+2]);
420       if (max_i > 0)
421          floor_ener -= MULT16_16(x[max_i-1], x[max_i-1]);
422       if (max_i > 1)
423          floor_ener -= MULT16_16(x[max_i-2], x[max_i-2]);
424       floor_ener = MAX32(floor_ener, EPSILON);
425 #endif
426       if (N>7 && eBands[i] >= m->pitchEnd)
427       {
428          celt_word16_t r;
429          celt_word16_t den = celt_sqrt(floor_ener);
430          den = MAX32(QCONST16(.02, 15), den);
431          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
432          ratio = ADD32(ratio, EXTEND32(r));
433          NR++;
434       }
435    }
436    if (NR>0)
437       ratio = DIV32_16(ratio, NR);
438    ratio = ADD32(HALF32(ratio), HALF32(*average));
439    if (!*last_decision)
440    {
441       *last_decision = (ratio < QCONST16(1.8,8));
442    } else {
443       *last_decision = (ratio < QCONST16(3.,8));
444    }
445    *average = EXTRACT16(ratio);
446    return *last_decision;
447 }
448
449 /* Quantisation of the residual */
450 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)
451 {
452    int i, j, remaining_bits, balance;
453    const celt_int16_t * restrict eBands = m->eBands;
454    celt_norm_t * restrict norm;
455    VARDECL(celt_norm_t, _norm);   const celt_int16_t *pBands = m->pBands;
456    int pband=-1;
457    int B;
458    SAVE_STACK;
459
460    B = shortBlocks ? m->nbShortMdcts : 1;
461    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
462    norm = _norm;
463
464    balance = 0;
465    for (i=0;i<m->nbEBands;i++)
466    {
467       int tell;
468       int N;
469       int q;
470       celt_word16_t n;
471       const celt_int16_t * const *BPbits;
472       
473       int curr_balance, curr_bits;
474       
475       N = eBands[i+1]-eBands[i];
476       BPbits = m->bits;
477
478       tell = ec_enc_tell(enc, BITRES);
479       if (i != 0)
480          balance -= tell;
481       remaining_bits = (total_bits<<BITRES)-tell-1;
482       curr_balance = (m->nbEBands-i);
483       if (curr_balance > 3)
484          curr_balance = 3;
485       curr_balance = balance / curr_balance;
486       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
487       curr_bits = pulses2bits(BPbits[i], N, q);
488       remaining_bits -= curr_bits;
489       while (remaining_bits < 0 && q > 0)
490       {
491          remaining_bits += curr_bits;
492          q--;
493          curr_bits = pulses2bits(BPbits[i], N, q);
494          remaining_bits -= curr_bits;
495       }
496       balance += pulses[i] + tell;
497       
498       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
499
500       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
501       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
502       {
503          int enabled = 1;
504          pband++;
505          if (remaining_bits >= 1<<BITRES) {
506            enabled = pgains[pband] > QCONST16(.5,15);
507            ec_enc_bits(enc, enabled, 1);
508            balance += 1<<BITRES;
509          }
510          if (enabled)
511             pgains[pband] = QCONST16(.9,15);
512          else
513             pgains[pband] = 0;
514       }
515
516       /* If pitch isn't available, use intra-frame prediction */
517       if (q==0)
518       {
519          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], norm, P+eBands[i], eBands[i], B);
520       } else if (pitch_used && eBands[i] < m->pitchEnd) {
521          for (j=eBands[i];j<eBands[i+1];j++)
522             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
523       } else {
524          for (j=eBands[i];j<eBands[i+1];j++)
525             P[j] = 0;
526       }
527       
528       if (q > 0)
529       {
530          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
531          alg_quant(X+eBands[i], W+eBands[i], eBands[i+1]-eBands[i], q, spread, P+eBands[i], enc);
532       } else {
533          for (j=eBands[i];j<eBands[i+1];j++)
534             X[j] = P[j];
535       }
536       for (j=eBands[i];j<eBands[i+1];j++)
537          norm[j] = MULT16_16_Q15(n,X[j]);
538    }
539    RESTORE_STACK;
540 }
541
542 #ifndef DISABLE_STEREO
543
544 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)
545 {
546    int i, j, remaining_bits, balance;
547    const celt_int16_t * restrict eBands = m->eBands;
548    celt_norm_t * restrict norm;
549    VARDECL(celt_norm_t, _norm);
550    const int C = CHANNELS(m);
551    const celt_int16_t *pBands = m->pBands;
552    int pband=-1;
553    int B;
554    celt_word16_t mid, side;
555    SAVE_STACK;
556
557    B = shortBlocks ? m->nbShortMdcts : 1;
558    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
559    norm = _norm;
560
561    balance = 0;
562    for (i=0;i<m->nbEBands;i++)
563    {
564       int c;
565       int tell;
566       int q1, q2;
567       celt_word16_t n;
568       const celt_int16_t * const *BPbits;
569       int b, qb;
570       int N;
571       int curr_balance, curr_bits;
572       int imid, iside, itheta;
573       int mbits, sbits, delta;
574       int qalloc;
575       
576       BPbits = m->bits;
577
578       N = eBands[i+1]-eBands[i];
579       tell = ec_enc_tell(enc, BITRES);
580       if (i != 0)
581          balance -= tell;
582       remaining_bits = (total_bits<<BITRES)-tell-1;
583       curr_balance = (m->nbEBands-i);
584       if (curr_balance > 3)
585          curr_balance = 3;
586       curr_balance = balance / curr_balance;
587       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
588       if (b<0)
589          b = 0;
590
591       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
592       if (qb > (b>>BITRES)-1)
593          qb = (b>>BITRES)-1;
594       if (qb<0)
595          qb = 0;
596       if (qb>14)
597          qb = 14;
598
599       stereo_band_mix(m, X, bandE, qb==0, i, 1);
600
601       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
602       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
603 #ifdef FIXED_POINT
604       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
605 #else
606       itheta = floor(.5+16384*0.63662*atan2(side,mid));
607 #endif
608       qalloc = log2_frac((1<<qb)+1,BITRES);
609       if (qb==0)
610       {
611          itheta=0;
612       } else {
613          int shift;
614          shift = 14-qb;
615          itheta = (itheta+(1<<shift>>1))>>shift;
616          ec_enc_uint(enc, itheta, (1<<qb)+1);
617          itheta <<= shift;
618       }
619       if (itheta == 0)
620       {
621          imid = 32767;
622          iside = 0;
623          delta = -10000;
624       } else if (itheta == 16384)
625       {
626          imid = 0;
627          iside = 32767;
628          delta = 10000;
629       } else {
630          imid = bitexact_cos(itheta);
631          iside = bitexact_cos(16384-itheta);
632          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
633       }
634       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
635
636       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
637       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
638       {
639          int enabled = 1;
640          pband++;
641          if (remaining_bits >= 1<<BITRES) {
642             enabled = pgains[pband] > QCONST16(.5,15);
643             ec_enc_bits(enc, enabled, 1);
644             balance += 1<<BITRES;
645             remaining_bits -= 1<<BITRES;
646          }
647          if (enabled)
648             pgains[pband] = QCONST16(.9,15);
649          else
650             pgains[pband] = 0;
651       }
652
653       if (N==2)
654       {
655          int c2;
656          int sign=1;
657          celt_norm_t v[2], w[2];
658          celt_norm_t *x2 = X+C*eBands[i];
659          mbits = b-qalloc;
660          sbits = 0;
661          if (itheta != 0 && itheta != 16384)
662             sbits = 1<<BITRES;
663          mbits -= sbits;
664          c = itheta > 8192 ? 1 : 0;
665          c2 = 1-c;
666
667          if (eBands[i] >= m->pitchEnd && fold)
668          {
669          } else if (pitch_used && eBands[i] < m->pitchEnd) {
670             stereo_band_mix(m, P, bandE, qb==0, i, 1);
671             renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
672             renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
673             deinterleave(P+C*eBands[i], C*N);
674             for (j=C*eBands[i];j<C*eBands[i+1];j++)
675                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
676          } else {
677             for (j=C*eBands[i];j<C*eBands[i+1];j++)
678                P[j] = 0;
679          }
680          v[0] = x2[c];
681          v[1] = x2[c+C];
682          w[0] = x2[c2];
683          w[1] = x2[c2+C];
684          q1 = bits2pulses(m, BPbits[i], N, mbits);
685          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
686          remaining_bits -= curr_bits;
687          while (remaining_bits < 0 && q1 > 0)
688          {
689             remaining_bits += curr_bits;
690             q1--;
691             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
692             remaining_bits -= curr_bits;
693          }
694
695          if (q1 > 0)
696          {
697             int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
698             alg_quant(v, W+C*eBands[i], N, q1, spread, P+C*eBands[i]+c*N, enc);
699          } else {
700             v[0] = QCONST16(1.f, 14);
701             v[1] = 0;
702          }
703          if (sbits)
704          {
705             if (v[0]*w[1] - v[1]*w[0] > 0)
706                sign = 1;
707             else
708                sign = -1;
709             ec_enc_bits(enc, sign==1, 1);
710          } else {
711             sign = 1;
712          }
713          w[0] = -sign*v[1];
714          w[1] = sign*v[0];
715          if (c==0)
716          {
717             x2[0] = v[0];
718             x2[1] = v[1];
719             x2[2] = w[0];
720             x2[3] = w[1];
721          } else {
722             x2[0] = w[0];
723             x2[1] = w[1];
724             x2[2] = v[0];
725             x2[3] = v[1];
726          }
727       } else {
728          
729       mbits = (b-qalloc/2-delta)/2;
730       if (mbits > b-qalloc)
731          mbits = b-qalloc;
732       if (mbits<0)
733          mbits=0;
734       sbits = b-qalloc-mbits;
735       q1 = bits2pulses(m, BPbits[i], N, mbits);
736       q2 = bits2pulses(m, BPbits[i], N, sbits);
737       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
738       remaining_bits -= curr_bits;
739       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
740       {
741          remaining_bits += curr_bits;
742          if (q1>q2)
743          {
744             q1--;
745             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
746          } else {
747             q2--;
748             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
749          }
750          remaining_bits -= curr_bits;
751       }
752
753       /* If pitch isn't available, use intra-frame prediction */
754       if (q1+q2==0)
755       {
756          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
757          deinterleave(P+C*eBands[i], C*N);
758       } else if (pitch_used && eBands[i] < m->pitchEnd) {
759          stereo_band_mix(m, P, bandE, qb==0, i, 1);
760          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
761          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
762          deinterleave(P+C*eBands[i], C*N);
763          for (j=C*eBands[i];j<C*eBands[i+1];j++)
764             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
765       } else {
766          for (j=C*eBands[i];j<C*eBands[i+1];j++)
767             P[j] = 0;
768       }
769       deinterleave(X+C*eBands[i], C*N);
770       if (q1 > 0) {
771          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
772          alg_quant(X+C*eBands[i], W+C*eBands[i], N, q1, spread, P+C*eBands[i], enc);
773       } else
774          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
775             X[j] = P[j];
776       if (q2 > 0) {
777          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
778          alg_quant(X+C*eBands[i]+N, W+C*eBands[i], N, q2, spread, P+C*eBands[i]+N, enc);
779       } else
780          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
781             X[j] = 0;
782       }
783       
784       balance += pulses[i] + tell;
785
786 #ifdef FIXED_POINT
787       mid = imid;
788       side = iside;
789 #else
790       mid = (1./32768)*imid;
791       side = (1./32768)*iside;
792 #endif
793       for (c=0;c<C;c++)
794          for (j=0;j<N;j++)
795             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
796
797       for (j=0;j<N;j++)
798          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
799       for (j=0;j<N;j++)
800          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
801
802       interleave(X+C*eBands[i], C*N);
803
804
805       stereo_band_mix(m, X, bandE, 0, i, -1);
806       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
807       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
808    }
809    RESTORE_STACK;
810 }
811 #endif /* DISABLE_STEREO */
812
813 /* Decoding of the residual */
814 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)
815 {
816    int i, j, remaining_bits, balance;
817    const celt_int16_t * restrict eBands = m->eBands;
818    celt_norm_t * restrict norm;
819    VARDECL(celt_norm_t, _norm);
820    const celt_int16_t *pBands = m->pBands;
821    int pband=-1;
822    int B;
823    SAVE_STACK;
824
825    B = shortBlocks ? m->nbShortMdcts : 1;
826    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
827    norm = _norm;
828
829    balance = 0;
830    for (i=0;i<m->nbEBands;i++)
831    {
832       int tell;
833       int N;
834       int q;
835       celt_word16_t n;
836       const celt_int16_t * const *BPbits;
837       
838       int curr_balance, curr_bits;
839
840       N = eBands[i+1]-eBands[i];
841       BPbits = m->bits;
842
843       tell = ec_dec_tell(dec, BITRES);
844       if (i != 0)
845          balance -= tell;
846       remaining_bits = (total_bits<<BITRES)-tell-1;
847       curr_balance = (m->nbEBands-i);
848       if (curr_balance > 3)
849          curr_balance = 3;
850       curr_balance = balance / curr_balance;
851       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
852       curr_bits = pulses2bits(BPbits[i], N, q);
853       remaining_bits -= curr_bits;
854       while (remaining_bits < 0 && q > 0)
855       {
856          remaining_bits += curr_bits;
857          q--;
858          curr_bits = pulses2bits(BPbits[i], N, q);
859          remaining_bits -= curr_bits;
860       }
861       balance += pulses[i] + tell;
862
863       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
864
865       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
866       if (pitch_used && eBands[i] < m->pitchEnd && eBands[i] == pBands[pband+1])
867       {
868          int enabled = 1;
869          pband++;
870          if (remaining_bits >= 1<<BITRES) {
871            enabled = ec_dec_bits(dec, 1);
872            balance += 1<<BITRES;
873          }
874          if (enabled)
875             pgains[pband] = QCONST16(.9,15);
876          else
877             pgains[pband] = 0;
878       }
879
880       /* If pitch isn't available, use intra-frame prediction */
881       if (q==0)
882       {
883          intra_fold(m, X+eBands[i], eBands[i+1]-eBands[i], norm, P+eBands[i], eBands[i], B);
884       } else if (pitch_used && eBands[i] < m->pitchEnd) {
885          for (j=eBands[i];j<eBands[i+1];j++)
886             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
887       } else {
888          for (j=eBands[i];j<eBands[i+1];j++)
889             P[j] = 0;
890       }
891       
892       if (q > 0)
893       {
894          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
895          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, P+eBands[i], dec);
896       } else {
897          for (j=eBands[i];j<eBands[i+1];j++)
898             X[j] = P[j];
899       }
900       for (j=eBands[i];j<eBands[i+1];j++)
901          norm[j] = MULT16_16_Q15(n,X[j]);
902    }
903    RESTORE_STACK;
904 }
905
906 #ifndef DISABLE_STEREO
907
908 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)
909 {
910    int i, j, remaining_bits, balance;
911    const celt_int16_t * restrict eBands = m->eBands;
912    celt_norm_t * restrict norm;
913    VARDECL(celt_norm_t, _norm);
914    const int C = CHANNELS(m);
915    const celt_int16_t *pBands = m->pBands;
916    int pband=-1;
917    int B;
918    celt_word16_t mid, side;
919    SAVE_STACK;
920
921    B = shortBlocks ? m->nbShortMdcts : 1;
922    ALLOC(_norm, C*eBands[m->nbEBands+1], celt_norm_t);
923    norm = _norm;
924
925    balance = 0;
926    for (i=0;i<m->nbEBands;i++)
927    {
928       int c;
929       int tell;
930       int q1, q2;
931       celt_word16_t n;
932       const celt_int16_t * const *BPbits;
933       int b, qb;
934       int N;
935       int curr_balance, curr_bits;
936       int imid, iside, itheta;
937       int mbits, sbits, delta;
938       int qalloc;
939       
940       BPbits = m->bits;
941
942       N = eBands[i+1]-eBands[i];
943       tell = ec_dec_tell(dec, BITRES);
944       if (i != 0)
945          balance -= tell;
946       remaining_bits = (total_bits<<BITRES)-tell-1;
947       curr_balance = (m->nbEBands-i);
948       if (curr_balance > 3)
949          curr_balance = 3;
950       curr_balance = balance / curr_balance;
951       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
952       if (b<0)
953          b = 0;
954
955       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
956       if (qb > (b>>BITRES)-1)
957          qb = (b>>BITRES)-1;
958       if (qb>14)
959          qb = 14;
960       if (qb<0)
961          qb = 0;
962       qalloc = log2_frac((1<<qb)+1,BITRES);
963       if (qb==0)
964       {
965          itheta=0;
966       } else {
967          int shift;
968          shift = 14-qb;
969          itheta = ec_dec_uint(dec, (1<<qb)+1);
970          itheta <<= shift;
971       }
972       if (itheta == 0)
973       {
974          imid = 32767;
975          iside = 0;
976          delta = -10000;
977       } else if (itheta == 16384)
978       {
979          imid = 0;
980          iside = 32767;
981          delta = 10000;
982       } else {
983          imid = bitexact_cos(itheta);
984          iside = bitexact_cos(16384-itheta);
985          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
986       }
987       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
988
989       /* If pitch is in use and this eBand begins a pitch band, encode the pitch gain flag */
990       if (pitch_used && eBands[i]< m->pitchEnd && eBands[i] == pBands[pband+1])
991       {
992          int enabled = 1;
993          pband++;
994          if (remaining_bits >= 1<<BITRES) {
995             enabled = ec_dec_bits(dec, 1);
996             balance += 1<<BITRES;
997             remaining_bits -= 1<<BITRES;
998          }
999          if (enabled)
1000             pgains[pband] = QCONST16(.9,15);
1001          else
1002             pgains[pband] = 0;
1003       }
1004
1005       if (N==2)
1006       {
1007          int c2;
1008          int sign=1;
1009          celt_norm_t v[2], w[2];
1010          celt_norm_t *x2 = X+C*eBands[i];
1011          mbits = b-qalloc;
1012          sbits = 0;
1013          if (itheta != 0 && itheta != 16384)
1014             sbits = 1<<BITRES;
1015          mbits -= sbits;
1016          c = itheta > 8192 ? 1 : 0;
1017          c2 = 1-c;
1018
1019          if (eBands[i] >= m->pitchEnd && fold)
1020          {
1021          } else if (pitch_used && eBands[i] < m->pitchEnd) {
1022             stereo_band_mix(m, P, bandE, qb==0, i, 1);
1023             renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1024             renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1025             deinterleave(P+C*eBands[i], C*N);
1026             for (j=C*eBands[i];j<C*eBands[i+1];j++)
1027                P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1028          } else {
1029             for (j=C*eBands[i];j<C*eBands[i+1];j++)
1030                P[j] = 0;
1031          }
1032          v[0] = x2[c];
1033          v[1] = x2[c+C];
1034          w[0] = x2[c2];
1035          w[1] = x2[c2+C];
1036          q1 = bits2pulses(m, BPbits[i], N, mbits);
1037          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
1038          remaining_bits -= curr_bits;
1039          while (remaining_bits < 0 && q1 > 0)
1040          {
1041             remaining_bits += curr_bits;
1042             q1--;
1043             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
1044             remaining_bits -= curr_bits;
1045          }
1046
1047          if (q1 > 0)
1048          {
1049             int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
1050             alg_unquant(v, N, q1, spread, P+C*eBands[i]+c*N, dec);
1051          } else {
1052             v[0] = QCONST16(1.f, 14);
1053             v[1] = 0;
1054          }
1055          if (sbits)
1056             sign = 2*ec_dec_bits(dec, 1)-1;
1057          else
1058             sign = 1;
1059          w[0] = -sign*v[1];
1060          w[1] = sign*v[0];
1061          if (c==0)
1062          {
1063             x2[0] = v[0];
1064             x2[1] = v[1];
1065             x2[2] = w[0];
1066             x2[3] = w[1];
1067          } else {
1068             x2[0] = w[0];
1069             x2[1] = w[1];
1070             x2[2] = v[0];
1071             x2[3] = v[1];
1072          }
1073       } else {
1074       mbits = (b-qalloc/2-delta)/2;
1075       if (mbits > b-qalloc)
1076          mbits = b-qalloc;
1077       if (mbits<0)
1078          mbits=0;
1079       sbits = b-qalloc-mbits;
1080       q1 = bits2pulses(m, BPbits[i], N, mbits);
1081       q2 = bits2pulses(m, BPbits[i], N, sbits);
1082       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1083       remaining_bits -= curr_bits;
1084       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
1085       {
1086          remaining_bits += curr_bits;
1087          if (q1>q2)
1088          {
1089             q1--;
1090             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1091          } else {
1092             q2--;
1093             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
1094          }
1095          remaining_bits -= curr_bits;
1096       }
1097       
1098
1099
1100       /* If pitch isn't available, use intra-frame prediction */
1101       if (q1+q2==0)
1102       {
1103          intra_fold(m, X+C*eBands[i], eBands[i+1]-eBands[i], norm, P+C*eBands[i], eBands[i], B);
1104          deinterleave(P+C*eBands[i], C*N);
1105       } else if (pitch_used && eBands[i] < m->pitchEnd) {
1106          stereo_band_mix(m, P, bandE, qb==0, i, 1);
1107          renormalise_vector(P+C*eBands[i], Q15ONE, N, C);
1108          renormalise_vector(P+C*eBands[i]+1, Q15ONE, N, C);
1109          deinterleave(P+C*eBands[i], C*N);
1110          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1111             P[j] = MULT16_16_Q15(pgains[pband], P[j]);
1112       } else {
1113          for (j=C*eBands[i];j<C*eBands[i+1];j++)
1114             P[j] = 0;
1115       }
1116       deinterleave(X+C*eBands[i], C*N);
1117       if (q1 > 0)
1118       {
1119          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
1120          alg_unquant(X+C*eBands[i], N, q1, spread, P+C*eBands[i], dec);
1121       } else
1122          for (j=C*eBands[i];j<C*eBands[i]+N;j++)
1123             X[j] = P[j];
1124       if (q2 > 0)
1125       {
1126          int spread = (eBands[i] >= m->pitchEnd && fold) ? B : 0;
1127          alg_unquant(X+C*eBands[i]+N, N, q2, spread, P+C*eBands[i]+N, dec);
1128       } else
1129          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
1130             X[j] = 0;
1131       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
1132       }
1133       balance += pulses[i] + tell;
1134
1135 #ifdef FIXED_POINT
1136       mid = imid;
1137       side = iside;
1138 #else
1139       mid = (1./32768)*imid;
1140       side = (1./32768)*iside;
1141 #endif
1142       for (c=0;c<C;c++)
1143          for (j=0;j<N;j++)
1144             norm[C*(eBands[i]+j)+c] = MULT16_16_Q15(n,X[C*eBands[i]+c*N+j]);
1145
1146       for (j=0;j<N;j++)
1147          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
1148       for (j=0;j<N;j++)
1149          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
1150
1151       interleave(X+C*eBands[i], C*N);
1152
1153       stereo_band_mix(m, X, bandE, 0, i, -1);
1154       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1155       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1156    }
1157    RESTORE_STACK;
1158 }
1159
1160 #endif /* DISABLE_STEREO */