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