Some work towards being able to encode a 48 kHz stream from 32 kHz audio (incomplete)
[opus.git] / libcelt / quant_bands.c
index caf3a20..af2b7e1 100644 (file)
@@ -1,5 +1,6 @@
-/* (C) 2007-2008 Jean-Marc Valin, CSIRO
-*/
+/* Copyright (c) 2007-2008 CSIRO
+   Copyright (c) 2007-2009 Xiph.Org Foundation
+   Written by Jean-Marc Valin */
 /*
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
 #include "mathops.h"
 #include "stack_alloc.h"
 
-#ifdef FIXED_POINT
-const celt_word16_t eMeans[24] = {1920, -341, -512, -107, 43, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-#else
-const celt_word16_t eMeans[24] = {7.5f, -1.33f, -2.f, -0.42f, 0.17f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
-#endif
+#define E_MEANS_SIZE (5)
+
+const celt_word16 eMeans[E_MEANS_SIZE] = {QCONST16(7.5f,DB_SHIFT), -QCONST16(1.33f,DB_SHIFT), -QCONST16(2.f,DB_SHIFT), -QCONST16(0.42f,DB_SHIFT), QCONST16(0.17f,DB_SHIFT)};
 
-int intra_decision(celt_word16_t *eBands, celt_word16_t *oldEBands, int len)
+/* FIXME: Implement for stereo */
+int intra_decision(celt_word16 *eBands, celt_word16 *oldEBands, int len)
 {
    int i;
-   celt_word32_t dist = 0;
+   celt_word32 dist = 0;
    for (i=0;i<len;i++)
    {
-      celt_word16_t d = SUB16(eBands[i], oldEBands[i]);
+      celt_word16 d = SUB16(eBands[i], oldEBands[i]);
       dist = MAC16_16(dist, d,d);
    }
-   return SHR32(dist,16) > 2*len;
+   return SHR32(dist,2*DB_SHIFT) > 2*len;
 }
 
 int *quant_prob_alloc(const CELTMode *m)
@@ -84,272 +84,242 @@ void quant_prob_free(int *freq)
    celt_free(freq);
 }
 
-static unsigned quant_coarse_energy_mono(const CELTMode *m, celt_word16_t *eBands, celt_word16_t *oldEBands, unsigned budget, int intra, int *prob, celt_word16_t *error, ec_enc *enc)
+unsigned quant_coarse_energy(const CELTMode *m, int start, int end, const celt_word16 *eBands, celt_word16 *oldEBands, int budget, int intra, int *prob, celt_word16 *error, ec_enc *enc, int _C, celt_word16 max_decay)
 {
-   int i;
-   unsigned bits;
+   int i, c;
    unsigned bits_used = 0;
-   celt_word16_t prev = 0;
-   celt_word16_t coef = m->ePredCoef;
-   celt_word16_t beta;
-   
+   celt_word32 prev[2] = {0,0};
+   celt_word16 coef = m->ePredCoef;
+   celt_word16 beta;
+   const int C = CHANNELS(_C);
+
    if (intra)
    {
       coef = 0;
       prob += 2*m->nbEBands;
    }
    /* The .8 is a heuristic */
-   beta = MULT16_16_Q15(QCONST16(.8f,15),coef);
-   
-   bits = ec_enc_tell(enc, 0);
+   beta = MULT16_16_P15(QCONST16(.8f,15),coef);
+
    /* Encode at a fixed coarse resolution */
-   for (i=0;i<m->nbEBands;i++)
+   for (i=start;i<end;i++)
    {
-      int qi;
-      celt_word16_t q;   /* dB */
-      celt_word16_t x;   /* dB */
-      celt_word16_t f;   /* Q8 */
-      celt_word16_t mean = MULT16_16_Q15(Q15ONE-coef,eMeans[i]);
-      x = eBands[i];
+      c=0;
+      do {
+         int qi;
+         celt_word16 q;
+         celt_word16 x;
+         celt_word32 f;
+         celt_word32 mean =  (i-start < E_MEANS_SIZE) ? SUB32(SHL32(EXTEND32(eMeans[i-start]),15), MULT16_16(coef,eMeans[i-start])) : 0;
+         x = eBands[i+c*m->nbEBands];
 #ifdef FIXED_POINT
-      f = x-mean -MULT16_16_Q15(coef,oldEBands[i])-prev;
-      /* Rounding to nearest integer here is really important! */
-      qi = (f+128)>>8;
+         f = SHL32(EXTEND32(x),15)-mean -MULT16_16(coef,oldEBands[i+c*m->nbEBands])-prev[c];
+         /* Rounding to nearest integer here is really important! */
+         qi = (f+QCONST32(.5,DB_SHIFT+15))>>(DB_SHIFT+15);
 #else
-      f = x-mean-coef*oldEBands[i]-prev;
-      /* Rounding to nearest integer here is really important! */
-      qi = (int)floor(.5f+f);
+         f = x-mean-coef*oldEBands[i+c*m->nbEBands]-prev[c];
+         /* Rounding to nearest integer here is really important! */
+         qi = (int)floor(.5f+f);
 #endif
-      /* If we don't have enough bits to encode all the energy, just assume something safe.
-         We allow slightly busting the budget here */
-      bits_used=ec_enc_tell(enc, 0) - bits;
-      if (bits_used > budget)
-      {
-         qi = -1;
-         error[i] = 128;
-      } else {
-         ec_laplace_encode_start(enc, &qi, prob[2*i], prob[2*i+1]);
-         error[i] = f - SHL16(qi,8);
-      }
-      q = qi*DB_SCALING;
-
-      oldEBands[i] = MULT16_16_Q15(coef,oldEBands[i])+(mean+prev+q);
-      prev = mean+prev+MULT16_16_Q15(Q15ONE-beta,q);
+         if (qi < 0 && x < oldEBands[i+c*m->nbEBands]-max_decay)
+         {
+            qi += SHR16(oldEBands[i+c*m->nbEBands]-max_decay-x, DB_SHIFT);
+            if (qi > 0)
+               qi = 0;
+         }
+         /* If we don't have enough bits to encode all the energy, just assume something safe.
+            We allow slightly busting the budget here */
+         bits_used=ec_enc_tell(enc, 0);
+         if (bits_used > budget)
+         {
+            qi = -1;
+            error[i+c*m->nbEBands] = QCONST16(.5f,DB_SHIFT);
+         } else {
+            ec_laplace_encode_start(enc, &qi, prob[2*i], prob[2*i+1]);
+            error[i+c*m->nbEBands] = PSHR32(f,15) - SHL16(qi,DB_SHIFT);
+         }
+         q = SHL16(qi,DB_SHIFT);
+         
+         oldEBands[i+c*m->nbEBands] = PSHR32(MULT16_16(coef,oldEBands[i+c*m->nbEBands]) + mean + prev[c] + SHL32(EXTEND32(q),15), 15);
+         prev[c] = mean + prev[c] + SHL32(EXTEND32(q),15) - MULT16_16(beta,q);
+      } while (++c < C);
    }
    return bits_used;
 }
 
-static void quant_fine_energy_mono(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, celt_word16_t *error, int *fine_quant, ec_enc *enc)
+void quant_fine_energy(const CELTMode *m, int start, int end, celt_ener *eBands, celt_word16 *oldEBands, celt_word16 *error, int *fine_quant, ec_enc *enc, int _C)
 {
-   int i;
+   int i, c;
+   const int C = CHANNELS(_C);
+
    /* Encode finer resolution */
-   for (i=0;i<m->nbEBands;i++)
+   for (i=start;i<end;i++)
    {
-      int q2;
-      celt_int16_t frac = 1<<fine_quant[i];
-      celt_word16_t offset;
+      celt_int16 frac = 1<<fine_quant[i];
       if (fine_quant[i] <= 0)
          continue;
+      c=0;
+      do {
+         int q2;
+         celt_word16 offset;
 #ifdef FIXED_POINT
-      /* Has to be without rounding */
-      q2 = (error[i]+QCONST16(.5f,8))>>(8-fine_quant[i]);
+         /* Has to be without rounding */
+         q2 = (error[i+c*m->nbEBands]+QCONST16(.5f,DB_SHIFT))>>(DB_SHIFT-fine_quant[i]);
 #else
-      q2 = (int)floor((error[i]+.5f)*frac);
+         q2 = (int)floor((error[i+c*m->nbEBands]+.5f)*frac);
 #endif
-      if (q2 > frac-1)
-         q2 = frac-1;
-      ec_enc_bits(enc, q2, fine_quant[i]);
+         if (q2 > frac-1)
+            q2 = frac-1;
+         if (q2<0)
+            q2 = 0;
+         ec_enc_bits(enc, q2, fine_quant[i]);
 #ifdef FIXED_POINT
-      offset = SUB16(SHR16(SHL16(q2,8)+QCONST16(.5,8),fine_quant[i]),QCONST16(.5f,8));
+         offset = SUB16(SHR16(SHL16(q2,DB_SHIFT)+QCONST16(.5,DB_SHIFT),fine_quant[i]),QCONST16(.5f,DB_SHIFT));
 #else
-      offset = (q2+.5f)*(1<<(14-fine_quant[i]))*(1.f/16384) - .5f;
+         offset = (q2+.5f)*(1<<(14-fine_quant[i]))*(1.f/16384) - .5f;
 #endif
-      oldEBands[i] += offset;
-      error[i] -= offset;
-      /*printf ("%f ", error[i] - offset);*/
+         oldEBands[i+c*m->nbEBands] += offset;
+         error[i+c*m->nbEBands] -= offset;
+         /*printf ("%f ", error[i] - offset);*/
+      } while (++c < C);
    }
-   for (i=0;i<m->nbEBands;i++)
-      eBands[i] = log2Amp(oldEBands[i]);
 }
 
-static void quant_energy_finalise_mono(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, celt_word16_t *error, int *fine_quant, int *fine_priority, int bits_left, ec_enc *enc)
+void quant_energy_finalise(const CELTMode *m, int start, int end, celt_ener *eBands, celt_word16 *oldEBands, celt_word16 *error, int *fine_quant, int *fine_priority, int bits_left, ec_enc *enc, int _C)
 {
-   int i, prio;
+   int i, prio, c;
+   const int C = CHANNELS(_C);
+
    /* Use up the remaining bits */
    for (prio=0;prio<2;prio++)
    {
-      for (i=0;i<m->nbEBands && bits_left!=0 ;i++)
+      for (i=start;i<end && bits_left>=C ;i++)
       {
-         int q2;
-         celt_word16_t offset;
          if (fine_quant[i] >= 7 || fine_priority[i]!=prio)
             continue;
-         q2 = error[i]<0 ? 0 : 1;
-         ec_enc_bits(enc, q2, 1);
+         c=0;
+         do {
+            int q2;
+            celt_word16 offset;
+            q2 = error[i+c*m->nbEBands]<0 ? 0 : 1;
+            ec_enc_bits(enc, q2, 1);
 #ifdef FIXED_POINT
-         offset = SHR16(SHL16(q2,8)-QCONST16(.5,8),fine_quant[i]+1);
+            offset = SHR16(SHL16(q2,DB_SHIFT)-QCONST16(.5,DB_SHIFT),fine_quant[i]+1);
 #else
-         offset = (q2-.5f)*(1<<(14-fine_quant[i]-1))*(1.f/16384);
+            offset = (q2-.5f)*(1<<(14-fine_quant[i]-1))*(1.f/16384);
 #endif
-         oldEBands[i] += offset;
-         bits_left--;
+            oldEBands[i+c*m->nbEBands] += offset;
+            bits_left--;
+         } while (++c < C);
       }
    }
-   for (i=0;i<m->nbEBands;i++)
-   {
-      eBands[i] = log2Amp(oldEBands[i]);
-      if (oldEBands[i] < -QCONST16(7.f,8))
-         oldEBands[i] = -QCONST16(7.f,8);
-   }
+   c=0;
+   do {
+      for (i=start;i<m->nbEBands;i++)
+      {
+         eBands[i+c*m->nbEBands] = log2Amp(oldEBands[i+c*m->nbEBands]);
+         if (oldEBands[i+c*m->nbEBands] < -QCONST16(7.f,DB_SHIFT))
+            oldEBands[i+c*m->nbEBands] = -QCONST16(7.f,DB_SHIFT);
+      }
+   } while (++c < C);
 }
 
-static void unquant_coarse_energy_mono(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, unsigned budget, int intra, int *prob, ec_dec *dec)
+void unquant_coarse_energy(const CELTMode *m, int start, int end, celt_ener *eBands, celt_word16 *oldEBands, int budget, int intra, int *prob, ec_dec *dec, int _C)
 {
-   int i;
-   unsigned bits;
-   celt_word16_t prev = 0;
-   celt_word16_t coef = m->ePredCoef;
-   celt_word16_t beta;
-   
+   int i, c;
+   celt_word32 prev[2] = {0, 0};
+   celt_word16 coef = m->ePredCoef;
+   celt_word16 beta;
+   const int C = CHANNELS(_C);
+
    if (intra)
    {
       coef = 0;
       prob += 2*m->nbEBands;
    }
    /* The .8 is a heuristic */
-   beta = MULT16_16_Q15(QCONST16(.8f,15),coef);
-   
-   bits = ec_dec_tell(dec, 0);
+   beta = MULT16_16_P15(QCONST16(.8f,15),coef);
+
    /* Decode at a fixed coarse resolution */
-   for (i=0;i<m->nbEBands;i++)
+   for (i=start;i<end;i++)
    {
-      int qi;
-      celt_word16_t q;
-      celt_word16_t mean = MULT16_16_Q15(Q15ONE-coef,eMeans[i]);
-      /* If we didn't have enough bits to encode all the energy, just assume something safe.
-         We allow slightly busting the budget here */
-      if (ec_dec_tell(dec, 0) - bits > budget)
-         qi = -1;
-      else
-         qi = ec_laplace_decode_start(dec, prob[2*i], prob[2*i+1]);
-      q = qi*DB_SCALING;
+      c=0;
+      do {
+         int qi;
+         celt_word16 q;
+         celt_word32 mean =  (i-start < E_MEANS_SIZE) ? SUB32(SHL32(EXTEND32(eMeans[i-start]),15), MULT16_16(coef,eMeans[i-start])) : 0;
+         /* If we didn't have enough bits to encode all the energy, just assume something safe.
+            We allow slightly busting the budget here */
+         if (ec_dec_tell(dec, 0) > budget)
+            qi = -1;
+         else
+            qi = ec_laplace_decode_start(dec, prob[2*i], prob[2*i+1]);
+         q = SHL16(qi,DB_SHIFT);
 
-      oldEBands[i] = MULT16_16_Q15(coef,oldEBands[i])+(mean+prev+q);
-      prev = mean+prev+MULT16_16_Q15(Q15ONE-beta,q);
+         oldEBands[i+c*m->nbEBands] = PSHR32(MULT16_16(coef,oldEBands[i+c*m->nbEBands]) + mean + prev[c] + SHL32(EXTEND32(q),15), 15);
+         prev[c] = mean + prev[c] + SHL32(EXTEND32(q),15) - MULT16_16(beta,q);
+      } while (++c < C);
    }
 }
 
-static void unquant_fine_energy_mono(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, int *fine_quant, ec_dec *dec)
+void unquant_fine_energy(const CELTMode *m, int start, int end, celt_ener *eBands, celt_word16 *oldEBands, int *fine_quant, ec_dec *dec, int _C)
 {
-   int i;
+   int i, c;
+   const int C = CHANNELS(_C);
    /* Decode finer resolution */
-   for (i=0;i<m->nbEBands;i++)
+   for (i=start;i<end;i++)
    {
-      int q2;
-      celt_word16_t offset;
       if (fine_quant[i] <= 0)
          continue;
-      q2 = ec_dec_bits(dec, fine_quant[i]);
+      c=0; 
+      do {
+         int q2;
+         celt_word16 offset;
+         q2 = ec_dec_bits(dec, fine_quant[i]);
 #ifdef FIXED_POINT
-      offset = SUB16(SHR16(SHL16(q2,8)+QCONST16(.5,8),fine_quant[i]),QCONST16(.5f,8));
+         offset = SUB16(SHR16(SHL16(q2,DB_SHIFT)+QCONST16(.5,DB_SHIFT),fine_quant[i]),QCONST16(.5f,DB_SHIFT));
 #else
-      offset = (q2+.5f)*(1<<(14-fine_quant[i]))*(1.f/16384) - .5f;
+         offset = (q2+.5f)*(1<<(14-fine_quant[i]))*(1.f/16384) - .5f;
 #endif
-      oldEBands[i] += offset;
+         oldEBands[i+c*m->nbEBands] += offset;
+      } while (++c < C);
    }
-   for (i=0;i<m->nbEBands;i++)
-      eBands[i] = log2Amp(oldEBands[i]);
 }
 
-static void unquant_energy_finalise_mono(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, int *fine_quant,  int *fine_priority, int bits_left, ec_dec *dec)
+void unquant_energy_finalise(const CELTMode *m, int start, int end, celt_ener *eBands, celt_word16 *oldEBands, int *fine_quant,  int *fine_priority, int bits_left, ec_dec *dec, int _C)
 {
-   int i, prio;
+   int i, prio, c;
+   const int C = CHANNELS(_C);
+
    /* Use up the remaining bits */
    for (prio=0;prio<2;prio++)
    {
-      for (i=0;i<m->nbEBands && bits_left!=0 ;i++)
+      for (i=start;i<end && bits_left>=C ;i++)
       {
-         int q2;
-         celt_word16_t offset;
          if (fine_quant[i] >= 7 || fine_priority[i]!=prio)
             continue;
-         q2 = ec_dec_bits(dec, 1);
+         c=0;
+         do {
+            int q2;
+            celt_word16 offset;
+            q2 = ec_dec_bits(dec, 1);
 #ifdef FIXED_POINT
-         offset = SHR16(SHL16(q2,8)-QCONST16(.5,8),fine_quant[i]+1);
+            offset = SHR16(SHL16(q2,DB_SHIFT)-QCONST16(.5,DB_SHIFT),fine_quant[i]+1);
 #else
-         offset = (q2-.5f)*(1<<(14-fine_quant[i]-1))*(1.f/16384);
+            offset = (q2-.5f)*(1<<(14-fine_quant[i]-1))*(1.f/16384);
 #endif
-         oldEBands[i] += offset;
-         bits_left--;
+            oldEBands[i+c*m->nbEBands] += offset;
+            bits_left--;
+         } while (++c < C);
       }
    }
-   for (i=0;i<m->nbEBands;i++)
-   {
-      eBands[i] = log2Amp(oldEBands[i]);
-      if (oldEBands[i] < -QCONST16(7.f,8))
-         oldEBands[i] = -QCONST16(7.f,8);
-   }
-}
-
-
-unsigned quant_coarse_energy(const CELTMode *m, celt_word16_t *eBands, celt_word16_t *oldEBands, int budget, int intra, int *prob, celt_word16_t *error, ec_enc *enc)
-{
-   int C, c;
-   unsigned maxBudget=0;
-   
-   C = m->nbChannels;
-
-   for (c=0;c<C;c++)
-   {
-      unsigned coarse_needed;
-      coarse_needed=quant_coarse_energy_mono(m, eBands+c*m->nbEBands, oldEBands+c*m->nbEBands, budget/C, intra, prob, error+c*m->nbEBands, enc);
-      maxBudget=IMAX(maxBudget,coarse_needed);
-   }
-   return maxBudget*C;
-}
-
-void quant_fine_energy(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, celt_word16_t *error, int *fine_quant, ec_enc *enc)
-{
-   int C, c;
-   C = m->nbChannels;
-
-   for (c=0;c<C;c++)
-      quant_fine_energy_mono(m, eBands+c*m->nbEBands, oldEBands+c*m->nbEBands, error+c*m->nbEBands, fine_quant, enc);
-}
-
-void quant_energy_finalise(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, celt_word16_t *error, int *fine_quant, int *fine_priority, int bits_left, ec_enc *enc)
-{
-   int C, c;
-   C = m->nbChannels;
-
-   for (c=0;c<C;c++)
-      quant_energy_finalise_mono(m, eBands+c*m->nbEBands, oldEBands+c*m->nbEBands, error+c*m->nbEBands, fine_quant, fine_priority, bits_left/C, enc);
-}
-
-void unquant_coarse_energy(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, int budget, int intra, int *prob, ec_dec *dec)
-{
-   int C, c;
-
-   C = m->nbChannels;
-   for (c=0;c<C;c++)
-      unquant_coarse_energy_mono(m, eBands+c*m->nbEBands, oldEBands+c*m->nbEBands, budget/C, intra, prob, dec);
-}
-
-void unquant_fine_energy(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, int *fine_quant, ec_dec *dec)
-{
-   int C, c;
-   C = m->nbChannels;
-
-   for (c=0;c<C;c++)
-      unquant_fine_energy_mono(m, eBands+c*m->nbEBands, oldEBands+c*m->nbEBands, fine_quant, dec);
-}
-
-void unquant_energy_finalise(const CELTMode *m, celt_ener_t *eBands, celt_word16_t *oldEBands, int *fine_quant, int *fine_priority, int bits_left, ec_dec *dec)
-{
-   int C, c;
-
-   C = m->nbChannels;
-
-   for (c=0;c<C;c++)
-      unquant_energy_finalise_mono(m, eBands+c*m->nbEBands, oldEBands+c*m->nbEBands, fine_quant, fine_priority, bits_left/C, dec);
+   c=0;
+   do {
+      for (i=start;i<m->nbEBands;i++)
+      {
+         eBands[i+c*m->nbEBands] = log2Amp(oldEBands[i+c*m->nbEBands]);
+         if (oldEBands[i+c*m->nbEBands] < -QCONST16(7.f,DB_SHIFT))
+            oldEBands[i+c*m->nbEBands] = -QCONST16(7.f,DB_SHIFT);
+      }
+   } while (++c < C);
 }