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 */