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