Demoved the divisions from the inner pitch prediction loops, bumped the version
[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_pitch_gain(const CELTMode *m, const celt_sig_t *X, const celt_sig_t *P, int norm_rate, int *gain_id)
220 {
221    int j, c;
222    celt_word16_t g;
223    celt_word16_t delta;
224    const int C = CHANNELS(m);
225    celt_word32_t Sxy=0, Sxx=0, Syy=0;
226    int len = m->pitchEnd;
227 #ifdef FIXED_POINT
228    int shift = 0;
229    celt_word32_t maxabs=0;
230    for (j=0;j<len;j++)
231    {
232       maxabs = MAX32(maxabs, ABS32(X[j]));
233       maxabs = MAX32(maxabs, ABS32(P[j]));
234    }
235    shift = celt_ilog2(maxabs)-12;
236    if (shift<0)
237       shift = 0;
238 #endif
239    delta = PDIV32_16(Q15ONE, len);
240    for (c=0;c<C;c++)
241    {
242       celt_word16_t gg = Q15ONE;
243       for (j=0;j<len;j++)
244       {
245          celt_word16_t Xj, Pj;
246          Xj = EXTRACT16(SHR32(X[C*j+c], shift));
247          Pj = MULT16_16_P15(gg,EXTRACT16(SHR32(P[C*j+c], shift)));
248          Sxy = MAC16_16(Sxy, Xj, Pj);
249          Sxx = MAC16_16(Sxx, Pj, Pj);
250          Syy = MAC16_16(Syy, Xj, Xj);
251          gg = SUB16(gg, delta);
252       }
253    }
254 #ifdef FIXED_POINT
255    {
256       celt_word32_t num, den;
257       celt_word16_t fact;
258       fact = MULT16_16(QCONST16(.04, 14), norm_rate);
259       if (fact < QCONST16(1., 14))
260          fact = QCONST16(1., 14);
261       num = Sxy;
262       den = EPSILON+Sxx+MULT16_32_Q15(QCONST16(.03,15),Syy);
263       shift = celt_ilog2(Sxy)-16;
264       if (shift < 0)
265          shift = 0;
266       g = DIV32(SHL32(SHR32(num,shift),14),SHR32(den,shift));
267       if (Sxy < MULT16_32_Q15(fact, MULT16_16(celt_sqrt(EPSILON+Sxx),celt_sqrt(EPSILON+Syy))))
268          g = 0;
269       /* This MUST round down so that we don't over-estimate the gain */
270       *gain_id = EXTRACT16(SHR32(MULT16_16(20,(g-QCONST16(.5,14))),14));
271    }
272 #else
273    {
274       float fact = .04*norm_rate;
275       if (fact < 1)
276          fact = 1;
277       g = Sxy/(.1+Sxx+.03*Syy);
278       if (Sxy < .5*fact*celt_sqrt(1+Sxx*Syy))
279          g = 0;
280       /* This MUST round down so that we don't over-estimate the gain */
281       *gain_id = floor(20*(g-.5));
282    }
283 #endif
284    if (*gain_id < 0)
285    {
286       *gain_id = 0;
287       return 0;
288    } else {
289       if (*gain_id > 15)
290          *gain_id = 15;
291       return 1;
292    }
293 }
294
295 void apply_pitch(const CELTMode *m, celt_sig_t *X, const celt_sig_t *P, int gain_id, int pred)
296 {
297    int j, c;
298    celt_word16_t gain;
299    celt_word16_t delta;
300    const int C = CHANNELS(m);
301    int len = m->pitchEnd;
302    
303    gain = ADD16(QCONST16(.5,14), MULT16_16_16(QCONST16(.05,14),gain_id));
304    delta = PDIV32_16(gain, len);
305    if (pred)
306       gain = -gain;
307    else
308       delta = -delta;
309    for (c=0;c<C;c++)
310    {
311       celt_word16_t gg = gain;
312       for (j=0;j<len;j++)
313       {
314          X[C*j+c] += SHL(MULT16_32_Q15(gg,P[C*j+c]),1);
315          gg = ADD16(gg, delta);
316       }
317    }
318 }
319
320 #ifndef DISABLE_STEREO
321
322 static void stereo_band_mix(const CELTMode *m, celt_norm_t *X, const celt_ener_t *bank, int stereo_mode, int bandID, int dir)
323 {
324    int i = bandID;
325    const celt_int16_t *eBands = m->eBands;
326    const int C = CHANNELS(m);
327    int j;
328    celt_word16_t a1, a2;
329    if (stereo_mode==0)
330    {
331       /* Do mid-side when not doing intensity stereo */
332       a1 = QCONST16(.70711f,14);
333       a2 = dir*QCONST16(.70711f,14);
334    } else {
335       celt_word16_t left, right;
336       celt_word16_t norm;
337 #ifdef FIXED_POINT
338       int shift = celt_zlog2(MAX32(bank[i], bank[i+m->nbEBands]))-13;
339 #endif
340       left = VSHR32(bank[i],shift);
341       right = VSHR32(bank[i+m->nbEBands],shift);
342       norm = EPSILON + celt_sqrt(EPSILON+MULT16_16(left,left)+MULT16_16(right,right));
343       a1 = DIV32_16(SHL32(EXTEND32(left),14),norm);
344       a2 = dir*DIV32_16(SHL32(EXTEND32(right),14),norm);
345    }
346    for (j=eBands[i];j<eBands[i+1];j++)
347    {
348       celt_norm_t r, l;
349       l = X[j*C];
350       r = X[j*C+1];
351       X[j*C] = MULT16_16_Q14(a1,l) + MULT16_16_Q14(a2,r);
352       X[j*C+1] = MULT16_16_Q14(a1,r) - MULT16_16_Q14(a2,l);
353    }
354 }
355
356
357 void interleave(celt_norm_t *x, int N)
358 {
359    int i;
360    VARDECL(celt_norm_t, tmp);
361    SAVE_STACK;
362    ALLOC(tmp, N, celt_norm_t);
363    
364    for (i=0;i<N;i++)
365       tmp[i] = x[i];
366    for (i=0;i<N>>1;i++)
367    {
368       x[i<<1] = tmp[i];
369       x[(i<<1)+1] = tmp[i+(N>>1)];
370    }
371    RESTORE_STACK;
372 }
373
374 void deinterleave(celt_norm_t *x, int N)
375 {
376    int i;
377    VARDECL(celt_norm_t, tmp);
378    SAVE_STACK;
379    ALLOC(tmp, N, celt_norm_t);
380    
381    for (i=0;i<N;i++)
382       tmp[i] = x[i];
383    for (i=0;i<N>>1;i++)
384    {
385       x[i] = tmp[i<<1];
386       x[i+(N>>1)] = tmp[(i<<1)+1];
387    }
388    RESTORE_STACK;
389 }
390
391 #endif /* DISABLE_STEREO */
392
393 int folding_decision(const CELTMode *m, celt_norm_t *X, celt_word16_t *average, int *last_decision)
394 {
395    int i, c;
396    int NR=0;
397    celt_word32_t ratio = EPSILON;
398    const int C = CHANNELS(m);
399    const celt_int16_t * restrict eBands = m->eBands;
400    for (c=0;c<C;c++)
401    {
402    for (i=0;i<m->nbEBands;i++)
403    {
404       int j, N;
405       int max_i=0;
406       celt_word16_t max_val=EPSILON;
407       celt_word32_t floor_ener=EPSILON;
408       celt_norm_t * restrict x = X+C*eBands[i]+c;
409       N = eBands[i+1]-eBands[i];
410       for (j=0;j<N;j++)
411       {
412          if (ABS16(x[C*j])>max_val)
413          {
414             max_val = ABS16(x[C*j]);
415             max_i = j;
416          }
417       }
418 #if 0
419       for (j=0;j<N;j++)
420       {
421          if (abs(j-max_i)>2)
422             floor_ener += x[j]*x[j];
423       }
424 #else
425       floor_ener = QCONST32(1.,28)-MULT16_16(max_val,max_val);
426       if (max_i < N-1)
427          floor_ener -= MULT16_16(x[C*(max_i+1)], x[C*(max_i+1)]);
428       if (max_i < N-2)
429          floor_ener -= MULT16_16(x[C*(max_i+2)], x[C*(max_i+2)]);
430       if (max_i > 0)
431          floor_ener -= MULT16_16(x[C*(max_i-1)], x[C*(max_i-1)]);
432       if (max_i > 1)
433          floor_ener -= MULT16_16(x[C*(max_i-2)], x[C*(max_i-2)]);
434       floor_ener = MAX32(floor_ener, EPSILON);
435 #endif
436       if (N>7)
437       {
438          celt_word16_t r;
439          celt_word16_t den = celt_sqrt(floor_ener);
440          den = MAX32(QCONST16(.02, 15), den);
441          r = DIV32_16(SHL32(EXTEND32(max_val),8),den);
442          ratio = ADD32(ratio, EXTEND32(r));
443          NR++;
444       }
445    }
446    }
447    if (NR>0)
448       ratio = DIV32_16(ratio, NR);
449    ratio = ADD32(HALF32(ratio), HALF32(*average));
450    if (!*last_decision)
451    {
452       *last_decision = (ratio < QCONST16(1.8,8));
453    } else {
454       *last_decision = (ratio < QCONST16(3.,8));
455    }
456    *average = EXTRACT16(ratio);
457    return *last_decision;
458 }
459
460 /* Quantisation of the residual */
461 void quant_bands(const CELTMode *m, celt_norm_t * restrict X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
462 {
463    int i, j, remaining_bits, balance;
464    const celt_int16_t * restrict eBands = m->eBands;
465    celt_norm_t * restrict norm;
466    VARDECL(celt_norm_t, _norm);
467    int B;
468    SAVE_STACK;
469
470    B = shortBlocks ? m->nbShortMdcts : 1;
471    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
472    norm = _norm;
473
474    balance = 0;
475    for (i=0;i<m->nbEBands;i++)
476    {
477       int tell;
478       int N;
479       int q;
480       celt_word16_t n;
481       const celt_int16_t * const *BPbits;
482       
483       int curr_balance, curr_bits;
484       
485       N = eBands[i+1]-eBands[i];
486       BPbits = m->bits;
487
488       tell = ec_enc_tell(enc, BITRES);
489       if (i != 0)
490          balance -= tell;
491       remaining_bits = (total_bits<<BITRES)-tell-1;
492       curr_balance = (m->nbEBands-i);
493       if (curr_balance > 3)
494          curr_balance = 3;
495       curr_balance = balance / curr_balance;
496       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
497       curr_bits = pulses2bits(BPbits[i], N, q);
498       remaining_bits -= curr_bits;
499       while (remaining_bits < 0 && q > 0)
500       {
501          remaining_bits += curr_bits;
502          q--;
503          curr_bits = pulses2bits(BPbits[i], N, q);
504          remaining_bits -= curr_bits;
505       }
506       balance += pulses[i] + tell;
507       
508       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
509
510       if (q > 0)
511       {
512          int spread = fold ? B : 0;
513          alg_quant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, enc);
514       } else {
515          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
516       }
517       for (j=eBands[i];j<eBands[i+1];j++)
518          norm[j] = MULT16_16_Q15(n,X[j]);
519    }
520    RESTORE_STACK;
521 }
522
523 #ifndef DISABLE_STEREO
524
525 void quant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_enc *enc)
526 {
527    int i, j, remaining_bits, balance;
528    const celt_int16_t * restrict eBands = m->eBands;
529    celt_norm_t * restrict norm;
530    VARDECL(celt_norm_t, _norm);
531    const int C = CHANNELS(m);
532    int B;
533    celt_word16_t mid, side;
534    SAVE_STACK;
535
536    B = shortBlocks ? m->nbShortMdcts : 1;
537    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
538    norm = _norm;
539
540    balance = 0;
541    for (i=0;i<m->nbEBands;i++)
542    {
543       int c;
544       int tell;
545       int q1, q2;
546       celt_word16_t n;
547       const celt_int16_t * const *BPbits;
548       int b, qb;
549       int N;
550       int curr_balance, curr_bits;
551       int imid, iside, itheta;
552       int mbits, sbits, delta;
553       int qalloc;
554       
555       BPbits = m->bits;
556
557       N = eBands[i+1]-eBands[i];
558       tell = ec_enc_tell(enc, BITRES);
559       if (i != 0)
560          balance -= tell;
561       remaining_bits = (total_bits<<BITRES)-tell-1;
562       curr_balance = (m->nbEBands-i);
563       if (curr_balance > 3)
564          curr_balance = 3;
565       curr_balance = balance / curr_balance;
566       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
567       if (b<0)
568          b = 0;
569
570       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
571       if (qb > (b>>BITRES)-1)
572          qb = (b>>BITRES)-1;
573       if (qb<0)
574          qb = 0;
575       if (qb>14)
576          qb = 14;
577
578       stereo_band_mix(m, X, bandE, qb==0, i, 1);
579
580       mid = renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
581       side = renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
582 #ifdef FIXED_POINT
583       itheta = MULT16_16_Q15(QCONST16(0.63662,15),celt_atan2p(side, mid));
584 #else
585       itheta = floor(.5+16384*0.63662*atan2(side,mid));
586 #endif
587       qalloc = log2_frac((1<<qb)+1,BITRES);
588       if (qb==0)
589       {
590          itheta=0;
591       } else {
592          int shift;
593          shift = 14-qb;
594          itheta = (itheta+(1<<shift>>1))>>shift;
595          ec_enc_uint(enc, itheta, (1<<qb)+1);
596          itheta <<= shift;
597       }
598       if (itheta == 0)
599       {
600          imid = 32767;
601          iside = 0;
602          delta = -10000;
603       } else if (itheta == 16384)
604       {
605          imid = 0;
606          iside = 32767;
607          delta = 10000;
608       } else {
609          imid = bitexact_cos(itheta);
610          iside = bitexact_cos(16384-itheta);
611          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
612       }
613       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
614
615       if (N==2)
616       {
617          int c2;
618          int sign=1;
619          celt_norm_t v[2], w[2];
620          celt_norm_t *x2 = X+C*eBands[i];
621          mbits = b-qalloc;
622          sbits = 0;
623          if (itheta != 0 && itheta != 16384)
624             sbits = 1<<BITRES;
625          mbits -= sbits;
626          c = itheta > 8192 ? 1 : 0;
627          c2 = 1-c;
628
629          v[0] = x2[c];
630          v[1] = x2[c+C];
631          w[0] = x2[c2];
632          w[1] = x2[c2+C];
633          q1 = bits2pulses(m, BPbits[i], N, mbits);
634          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
635          remaining_bits -= curr_bits;
636          while (remaining_bits < 0 && q1 > 0)
637          {
638             remaining_bits += curr_bits;
639             q1--;
640             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
641             remaining_bits -= curr_bits;
642          }
643
644          if (q1 > 0)
645          {
646             int spread = fold ? B : 0;
647             alg_quant(v, N, q1, spread, enc);
648          } else {
649             v[0] = QCONST16(1.f, 14);
650             v[1] = 0;
651          }
652          if (sbits)
653          {
654             if (v[0]*w[1] - v[1]*w[0] > 0)
655                sign = 1;
656             else
657                sign = -1;
658             ec_enc_bits(enc, sign==1, 1);
659          } else {
660             sign = 1;
661          }
662          w[0] = -sign*v[1];
663          w[1] = sign*v[0];
664          if (c==0)
665          {
666             x2[0] = v[0];
667             x2[1] = v[1];
668             x2[2] = w[0];
669             x2[3] = w[1];
670          } else {
671             x2[0] = w[0];
672             x2[1] = w[1];
673             x2[2] = v[0];
674             x2[3] = v[1];
675          }
676       } else {
677          
678       mbits = (b-qalloc/2-delta)/2;
679       if (mbits > b-qalloc)
680          mbits = b-qalloc;
681       if (mbits<0)
682          mbits=0;
683       sbits = b-qalloc-mbits;
684       q1 = bits2pulses(m, BPbits[i], N, mbits);
685       q2 = bits2pulses(m, BPbits[i], N, sbits);
686       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
687       remaining_bits -= curr_bits;
688       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
689       {
690          remaining_bits += curr_bits;
691          if (q1>q2)
692          {
693             q1--;
694             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
695          } else {
696             q2--;
697             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
698          }
699          remaining_bits -= curr_bits;
700       }
701
702       deinterleave(X+C*eBands[i], C*N);
703       if (q1 > 0) {
704          int spread = fold ? B : 0;
705          alg_quant(X+C*eBands[i], N, q1, spread, enc);
706       } else {
707          intra_fold(m, eBands[i+1]-eBands[i], norm, X+C*eBands[i], eBands[i], B);
708       }
709       if (q2 > 0) {
710          int spread = fold ? B : 0;
711          alg_quant(X+C*eBands[i]+N, N, q2, spread, enc);
712       } else
713          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
714             X[j] = 0;
715       }
716       
717       balance += pulses[i] + tell;
718
719 #ifdef FIXED_POINT
720       mid = imid;
721       side = iside;
722 #else
723       mid = (1./32768)*imid;
724       side = (1./32768)*iside;
725 #endif
726       for (j=0;j<N;j++)
727          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
728
729       for (j=0;j<N;j++)
730          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
731       for (j=0;j<N;j++)
732          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
733
734       interleave(X+C*eBands[i], C*N);
735
736
737       stereo_band_mix(m, X, bandE, 0, i, -1);
738       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
739       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
740    }
741    RESTORE_STACK;
742 }
743 #endif /* DISABLE_STEREO */
744
745 /* Decoding of the residual */
746 void unquant_bands(const CELTMode *m, celt_norm_t * restrict X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
747 {
748    int i, j, remaining_bits, balance;
749    const celt_int16_t * restrict eBands = m->eBands;
750    celt_norm_t * restrict norm;
751    VARDECL(celt_norm_t, _norm);
752    int B;
753    SAVE_STACK;
754
755    B = shortBlocks ? m->nbShortMdcts : 1;
756    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
757    norm = _norm;
758
759    balance = 0;
760    for (i=0;i<m->nbEBands;i++)
761    {
762       int tell;
763       int N;
764       int q;
765       celt_word16_t n;
766       const celt_int16_t * const *BPbits;
767       
768       int curr_balance, curr_bits;
769
770       N = eBands[i+1]-eBands[i];
771       BPbits = m->bits;
772
773       tell = ec_dec_tell(dec, BITRES);
774       if (i != 0)
775          balance -= tell;
776       remaining_bits = (total_bits<<BITRES)-tell-1;
777       curr_balance = (m->nbEBands-i);
778       if (curr_balance > 3)
779          curr_balance = 3;
780       curr_balance = balance / curr_balance;
781       q = bits2pulses(m, BPbits[i], N, pulses[i]+curr_balance);
782       curr_bits = pulses2bits(BPbits[i], N, q);
783       remaining_bits -= curr_bits;
784       while (remaining_bits < 0 && q > 0)
785       {
786          remaining_bits += curr_bits;
787          q--;
788          curr_bits = pulses2bits(BPbits[i], N, q);
789          remaining_bits -= curr_bits;
790       }
791       balance += pulses[i] + tell;
792
793       n = SHL16(celt_sqrt(eBands[i+1]-eBands[i]),11);
794
795       if (q > 0)
796       {
797          int spread = fold ? B : 0;
798          alg_unquant(X+eBands[i], eBands[i+1]-eBands[i], q, spread, dec);
799       } else {
800          intra_fold(m, eBands[i+1]-eBands[i], norm, X+eBands[i], eBands[i], B);
801       }
802       for (j=eBands[i];j<eBands[i+1];j++)
803          norm[j] = MULT16_16_Q15(n,X[j]);
804    }
805    RESTORE_STACK;
806 }
807
808 #ifndef DISABLE_STEREO
809
810 void unquant_bands_stereo(const CELTMode *m, celt_norm_t * restrict X, const celt_ener_t *bandE, int *pulses, int shortBlocks, int fold, int total_bits, ec_dec *dec)
811 {
812    int i, j, remaining_bits, balance;
813    const celt_int16_t * restrict eBands = m->eBands;
814    celt_norm_t * restrict norm;
815    VARDECL(celt_norm_t, _norm);
816    const int C = CHANNELS(m);
817    int B;
818    celt_word16_t mid, side;
819    SAVE_STACK;
820
821    B = shortBlocks ? m->nbShortMdcts : 1;
822    ALLOC(_norm, eBands[m->nbEBands+1], celt_norm_t);
823    norm = _norm;
824
825    balance = 0;
826    for (i=0;i<m->nbEBands;i++)
827    {
828       int c;
829       int tell;
830       int q1, q2;
831       celt_word16_t n;
832       const celt_int16_t * const *BPbits;
833       int b, qb;
834       int N;
835       int curr_balance, curr_bits;
836       int imid, iside, itheta;
837       int mbits, sbits, delta;
838       int qalloc;
839       
840       BPbits = m->bits;
841
842       N = eBands[i+1]-eBands[i];
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       b = IMIN(remaining_bits+1,pulses[i]+curr_balance);
852       if (b<0)
853          b = 0;
854
855       qb = (b-2*(N-1)*(QTHETA_OFFSET-log2_frac(N,BITRES)))/(32*(N-1));
856       if (qb > (b>>BITRES)-1)
857          qb = (b>>BITRES)-1;
858       if (qb>14)
859          qb = 14;
860       if (qb<0)
861          qb = 0;
862       qalloc = log2_frac((1<<qb)+1,BITRES);
863       if (qb==0)
864       {
865          itheta=0;
866       } else {
867          int shift;
868          shift = 14-qb;
869          itheta = ec_dec_uint(dec, (1<<qb)+1);
870          itheta <<= shift;
871       }
872       if (itheta == 0)
873       {
874          imid = 32767;
875          iside = 0;
876          delta = -10000;
877       } else if (itheta == 16384)
878       {
879          imid = 0;
880          iside = 32767;
881          delta = 10000;
882       } else {
883          imid = bitexact_cos(itheta);
884          iside = bitexact_cos(16384-itheta);
885          delta = (N-1)*(log2_frac(iside,BITRES+2)-log2_frac(imid,BITRES+2))>>2;
886       }
887       n = SHL16(celt_sqrt((eBands[i+1]-eBands[i])),11);
888
889       if (N==2)
890       {
891          int c2;
892          int sign=1;
893          celt_norm_t v[2], w[2];
894          celt_norm_t *x2 = X+C*eBands[i];
895          mbits = b-qalloc;
896          sbits = 0;
897          if (itheta != 0 && itheta != 16384)
898             sbits = 1<<BITRES;
899          mbits -= sbits;
900          c = itheta > 8192 ? 1 : 0;
901          c2 = 1-c;
902
903          v[0] = x2[c];
904          v[1] = x2[c+C];
905          w[0] = x2[c2];
906          w[1] = x2[c2+C];
907          q1 = bits2pulses(m, BPbits[i], N, mbits);
908          curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc+sbits;
909          remaining_bits -= curr_bits;
910          while (remaining_bits < 0 && q1 > 0)
911          {
912             remaining_bits += curr_bits;
913             q1--;
914             curr_bits = pulses2bits(BPbits[i], N, q1)+qalloc;
915             remaining_bits -= curr_bits;
916          }
917
918          if (q1 > 0)
919          {
920             int spread = fold ? B : 0;
921             alg_unquant(v, N, q1, spread, dec);
922          } else {
923             v[0] = QCONST16(1.f, 14);
924             v[1] = 0;
925          }
926          if (sbits)
927             sign = 2*ec_dec_bits(dec, 1)-1;
928          else
929             sign = 1;
930          w[0] = -sign*v[1];
931          w[1] = sign*v[0];
932          if (c==0)
933          {
934             x2[0] = v[0];
935             x2[1] = v[1];
936             x2[2] = w[0];
937             x2[3] = w[1];
938          } else {
939             x2[0] = w[0];
940             x2[1] = w[1];
941             x2[2] = v[0];
942             x2[3] = v[1];
943          }
944       } else {
945       mbits = (b-qalloc/2-delta)/2;
946       if (mbits > b-qalloc)
947          mbits = b-qalloc;
948       if (mbits<0)
949          mbits=0;
950       sbits = b-qalloc-mbits;
951       q1 = bits2pulses(m, BPbits[i], N, mbits);
952       q2 = bits2pulses(m, BPbits[i], N, sbits);
953       curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
954       remaining_bits -= curr_bits;
955       while (remaining_bits < 0 && (q1 > 0 || q2 > 0))
956       {
957          remaining_bits += curr_bits;
958          if (q1>q2)
959          {
960             q1--;
961             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
962          } else {
963             q2--;
964             curr_bits = pulses2bits(BPbits[i], N, q1)+pulses2bits(BPbits[i], N, q2)+qalloc;
965          }
966          remaining_bits -= curr_bits;
967       }
968
969       deinterleave(X+C*eBands[i], C*N);
970       if (q1 > 0)
971       {
972          int spread = fold ? B : 0;
973          alg_unquant(X+C*eBands[i], N, q1, spread, dec);
974       } else
975          intra_fold(m, eBands[i+1]-eBands[i], norm, X+C*eBands[i], eBands[i], B);
976       if (q2 > 0)
977       {
978          int spread = fold ? B : 0;
979          alg_unquant(X+C*eBands[i]+N, N, q2, spread, dec);
980       } else
981          for (j=C*eBands[i]+N;j<C*eBands[i+1];j++)
982             X[j] = 0;
983       /*orthogonalize(X+C*eBands[i], X+C*eBands[i]+N, N);*/
984       }
985       balance += pulses[i] + tell;
986
987 #ifdef FIXED_POINT
988       mid = imid;
989       side = iside;
990 #else
991       mid = (1./32768)*imid;
992       side = (1./32768)*iside;
993 #endif
994       for (j=0;j<N;j++)
995          norm[eBands[i]+j] = MULT16_16_Q15(n,X[C*eBands[i]+j]);
996
997       for (j=0;j<N;j++)
998          X[C*eBands[i]+j] = MULT16_16_Q15(X[C*eBands[i]+j], mid);
999       for (j=0;j<N;j++)
1000          X[C*eBands[i]+N+j] = MULT16_16_Q15(X[C*eBands[i]+N+j], side);
1001
1002       interleave(X+C*eBands[i], C*N);
1003
1004       stereo_band_mix(m, X, bandE, 0, i, -1);
1005       renormalise_vector(X+C*eBands[i], Q15ONE, N, C);
1006       renormalise_vector(X+C*eBands[i]+1, Q15ONE, N, C);
1007    }
1008    RESTORE_STACK;
1009 }
1010
1011 #endif /* DISABLE_STEREO */