nb_celp.c

ppciaxclient softphone

/* Copyright (C) 2002 Jean-Marc Valin 
   File: nb_celp.c

   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions
   are met:
   
   - Redistributions of source code must retain the above copyright
   notice, this list of conditions and the following disclaimer.
   
   - Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.
   
   - Neither the name of the Xiph.org Foundation nor the names of its
   contributors may be used to endorse or promote products derived from
   this software without specific prior written permission.
   
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <math.h>
#include "nb_celp.h"
#include "lpc.h"
#include "lsp.h"
#include "ltp.h"
#include "quant_lsp.h"
#include "cb_search.h"
#include "filters.h"
#include "stack_alloc.h"
#include "vq.h"
#include <speex/speex_bits.h>
#include "vbr.h"
#include "misc.h"
#include <speex/speex_callbacks.h>

#ifndef M_PI
#define M_PI           3.14159265358979323846  /* pi */
#endif

#ifndef NULL
#define NULL 0
#endif

#define SUBMODE(x) st->submodes[st->submodeID]->x


#ifdef FIXED_POINT
const spx_word32_t ol_gain_table[32]={18900, 25150, 33468, 44536, 59265, 78865, 104946, 139653, 185838, 247297, 329081, 437913, 582736, 775454, 1031906, 1373169, 1827293, 2431601, 3235761, 4305867, 5729870, 7624808, 10146425, 13501971, 17967238, 23909222, 31816294, 42338330, 56340132, 74972501, 99766822, 132760927};
const spx_word16_t exc_gain_quant_scal3_bound[7]={1841, 3883, 6051, 8062, 10444, 13580, 18560};
const spx_word16_t exc_gain_quant_scal3[8]={1002, 2680, 5086, 7016, 9108, 11781, 15380, 21740};
const spx_word16_t exc_gain_quant_scal1_bound[1]={14385};
const spx_word16_t exc_gain_quant_scal1[2]={11546, 17224};

#define LSP_MARGIN 16
#define LSP_DELTA1 6553
#define LSP_DELTA2 1638

#else

const float exc_gain_quant_scal3_bound[7]={0.112338, 0.236980, 0.369316, 0.492054, 0.637471, 0.828874, 1.132784};
const float exc_gain_quant_scal3[8]={0.061130, 0.163546, 0.310413, 0.428220, 0.555887, 0.719055, 0.938694, 1.326874};
const float exc_gain_quant_scal1_bound[1]={0.87798};
const float exc_gain_quant_scal1[2]={0.70469, 1.05127};

#define LSP_MARGIN .002
#define LSP_DELTA1 .2
#define LSP_DELTA2 .05

#endif




#define sqr(x) ((x)*(x))

void *nb_encoder_init(const SpeexMode *m)
{
   EncState *st;
   const SpeexNBMode *mode;
   int i;

   mode=(const SpeexNBMode *)m->mode;
#if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
   st = (EncState*)speex_alloc(sizeof(EncState));
   st->stack = NULL;
#else
   st = (EncState*)speex_alloc(sizeof(EncState)+8000*sizeof(spx_sig_t));
   st->stack = ((char*)st) + sizeof(EncState);
#endif
   if (!st)
      return NULL;
   
   st->mode=m;

   st->frameSize = mode->frameSize;
   st->windowSize = st->frameSize*3/2;
   st->nbSubframes=mode->frameSize/mode->subframeSize;
   st->subframeSize=mode->subframeSize;
   st->lpcSize = mode->lpcSize;
   st->gamma1=mode->gamma1;
   st->gamma2=mode->gamma2;
   st->min_pitch=mode->pitchStart;
   st->max_pitch=mode->pitchEnd;
   st->lag_factor=mode->lag_factor;
   st->lpc_floor = mode->lpc_floor;
  
   st->submodes=mode->submodes;
   st->submodeID=st->submodeSelect=mode->defaultSubmode;
   st->bounded_pitch = 1;

   st->encode_submode = 1;
#ifdef EPIC_48K
   st->lbr_48k=mode->lbr48k;
#endif

   /* Allocating input buffer */
   st->inBuf = speex_alloc((st->windowSize)*sizeof(spx_sig_t));
   st->frame = st->inBuf;
   /* Allocating excitation buffer */
   st->excBuf = speex_alloc((mode->frameSize+mode->pitchEnd+1)*sizeof(spx_sig_t));
   st->exc = st->excBuf + mode->pitchEnd + 1;
   st->swBuf = speex_alloc((mode->frameSize+mode->pitchEnd+1)*sizeof(spx_sig_t));
   st->sw = st->swBuf + mode->pitchEnd + 1;

   st->innov = speex_alloc((st->frameSize)*sizeof(spx_sig_t));

   /* Asymmetric "pseudo-Hamming" window */
   {
      int part1, part2;
      part1=st->frameSize - (st->subframeSize>>1);
      part2=(st->frameSize>>1) + (st->subframeSize>>1);
      st->window = speex_alloc((st->windowSize)*sizeof(spx_word16_t));
      for (i=0;i<part1;i++)
         st->window[i]=(spx_word16_t)(SIG_SCALING*(.54-.46*cos(M_PI*i/part1)));
      for (i=0;i<part2;i++)
         st->window[part1+i]=(spx_word16_t)(SIG_SCALING*(.54+.46*cos(M_PI*i/part2)));
   }
   /* Create the window for autocorrelation (lag-windowing) */
   st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
   for (i=0;i<st->lpcSize+1;i++)
      st->lagWindow[i]=16384*exp(-.5*sqr(2*M_PI*st->lag_factor*i));

   st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));

   st->lpc = speex_alloc((st->lpcSize+1)*sizeof(spx_coef_t));
   st->interp_lpc = speex_alloc((st->lpcSize+1)*sizeof(spx_coef_t));
   st->interp_qlpc = speex_alloc((st->lpcSize+1)*sizeof(spx_coef_t));
   st->bw_lpc1 = speex_alloc((st->lpcSize+1)*sizeof(spx_coef_t));
   st->bw_lpc2 = speex_alloc((st->lpcSize+1)*sizeof(spx_coef_t));

   st->lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->old_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->old_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->interp_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
   st->interp_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));

   st->first = 1;
   for (i=0;i<st->lpcSize;i++)
   {
      st->lsp[i]=LSP_SCALING*(M_PI*((float)(i+1)))/(st->lpcSize+1);
   }

   st->mem_sp = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
   st->mem_sw = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
   st->mem_sw_whole = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
   st->mem_exc = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));

   st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));

   st->pitch = speex_alloc((st->nbSubframes)*sizeof(int));

   st->vbr = speex_alloc(sizeof(VBRState));
   vbr_init(st->vbr);
   st->vbr_quality = 8;
   st->vbr_enabled = 0;
   st->vad_enabled = 0;
   st->dtx_enabled = 0;
   st->abr_enabled = 0;
   st->abr_drift = 0;

   st->plc_tuning = 2;
   st->complexity=2;
   st->sampling_rate=8000;
   st->dtx_count=0;

#ifdef ENABLE_VALGRIND
   VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
#endif
   return st;
}

void nb_encoder_destroy(void *state)
{
   EncState *st=(EncState *)state;
   /* Free all allocated memory */

   vbr_destroy(st->vbr);

   /*Free state memory... should be last*/
   speex_free(st);
}

int nb_encode(void *state, void *vin, SpeexBits *bits)
{
   EncState *st;
   int i, sub, roots;
   int ol_pitch;
   spx_word16_t ol_pitch_coef;
   spx_word32_t ol_gain;
   VARDECL(spx_sig_t *res);
   VARDECL(spx_sig_t *target);
   VARDECL(spx_mem_t *mem);
   char *stack;
   VARDECL(spx_word16_t *syn_resp);
   VARDECL(spx_sig_t *real_exc);
#ifdef EPIC_48K
   int pitch_half[2];
   int ol_pitch_id=0;
#endif
   spx_word16_t *in = vin;

   st=(EncState *)state;
   stack=st->stack;

   /* Copy new data in input buffer */
   speex_move(st->inBuf, st->inBuf+st->frameSize, (st->windowSize-st->frameSize)*sizeof(spx_sig_t));
   for (i=0;i<st->frameSize;i++)
      st->inBuf[st->windowSize-st->frameSize+i] = SHL32(EXTEND32(in[i]), SIG_SHIFT);

   /* Move signals 1 frame towards the past */
   speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch+1)*sizeof(spx_sig_t));
   speex_move(st->swBuf, st->swBuf+st->frameSize, (st->max_pitch+1)*sizeof(spx_sig_t));

   {
      VARDECL(spx_word16_t *w_sig);
      ALLOC(w_sig, st->windowSize, spx_word16_t);
      /* Window for analysis */
      for (i=0;i<st->windowSize;i++)
         w_sig[i] = EXTRACT16(SHR32(MULT16_16(EXTRACT16(SHR32(st->frame[i],SIG_SHIFT)),st->window[i]),SIG_SHIFT));

      /* Compute auto-correlation */
      _spx_autocorr(w_sig, st->autocorr, st->lpcSize+1, st->windowSize);
   }
   st->autocorr[0] = (spx_word16_t) (st->autocorr[0]*st->lpc_floor); /* Noise floor in auto-correlation domain */

   /* Lag windowing: equivalent to filtering in the power-spectrum domain */
   for (i=0;i<st->lpcSize+1;i++)
      st->autocorr[i] = MULT16_16_Q14(st->autocorr[i],st->lagWindow[i]);

   /* Levinson-Durbin */
   _spx_lpc(st->lpc+1, st->autocorr, st->lpcSize);
   st->lpc[0]=(spx_coef_t)LPC_SCALING;

   /* LPC to LSPs (x-domain) transform */
   roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, LSP_DELTA1, stack);
   /* Check if we found all the roots */
   if (roots!=st->lpcSize)
   {
      /* Search again if we can afford it */
      if (st->complexity>1)
         roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, LSP_DELTA2, stack);
      if (roots!=st->lpcSize) 
      {
         /*If we can't find all LSP's, do some damage control and use previous filter*/
         for (i=0;i<st->lpcSize;i++)
         {
            st->lsp[i]=st->old_lsp[i];
         }
      }
   }



   /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
   {
      if (st->first)
         for (i=0;i<st->lpcSize;i++)
            st->interp_lsp[i] = st->lsp[i];
      else
         lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, st->nbSubframes, st->nbSubframes<<1);

      lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN);

      /* Compute interpolated LPCs (unquantized) for whole frame*/
      lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);


      /*Open-loop pitch*/
      if (!st->submodes[st->submodeID] || st->vbr_enabled || st->vad_enabled || SUBMODE(forced_pitch_gain) ||
          SUBMODE(lbr_pitch) != -1)
      {
         int nol_pitch[6];
         spx_word16_t nol_pitch_coef[6];
         
         bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
         bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
         
         filter_mem2(st->frame, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);

         open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
                               nol_pitch, nol_pitch_coef, 6, stack);
         ol_pitch=nol_pitch[0];
         ol_pitch_coef = nol_pitch_coef[0];
         /*Try to remove pitch multiples*/
         for (i=1;i<6;i++)
         {
#ifdef FIXED_POINT
            if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],27853)) && 
#else
            if ((nol_pitch_coef[i]>.85*nol_pitch_coef[0]) && 
#endif
                (ABS(2*nol_pitch[i]-ol_pitch)<=2 || ABS(3*nol_pitch[i]-ol_pitch)<=3 || 
                 ABS(4*nol_pitch[i]-ol_pitch)<=4 || ABS(5*nol_pitch[i]-ol_pitch)<=5))
            {
               /*ol_pitch_coef=nol_pitch_coef[i];*/
               ol_pitch = nol_pitch[i];
            }
         }
         /*if (ol_pitch>50)
           ol_pitch/=2;*/
         /*ol_pitch_coef = sqrt(ol_pitch_coef);*/

#ifdef EPIC_48K
         if (st->lbr_48k)
         {
            if (ol_pitch < st->min_pitch+2)
               ol_pitch = st->min_pitch+2;
            if (ol_pitch > st->max_pitch-2)
               ol_pitch = st->max_pitch-2;
            open_loop_nbest_pitch(st->sw, ol_pitch-2, ol_pitch+2, st->frameSize>>1, 
                                  &pitch_half[0], nol_pitch_coef, 1, stack);
            open_loop_nbest_pitch(st->sw+(st->frameSize>>1), pitch_half[0]-1, pitch_half[0]+2, st->frameSize>>1, 
                                  &pitch_half[1], nol_pitch_coef, 1, stack);
         }
#endif
      } else {
         ol_pitch=0;
         ol_pitch_coef=0;
      }
      /*Compute "real" excitation*/
      fir_mem2(st->frame, st->interp_lpc, st->exc, st->frameSize, st->lpcSize, st->mem_exc);

      /* Compute open-loop excitation gain */
#ifdef EPIC_48K
      if (st->lbr_48k)
      {
         float ol1=0,ol2=0;
         float ol_gain2;
         ol1 = compute_rms(st->exc, st->frameSize>>1);
         ol2 = compute_rms(st->exc+(st->frameSize>>1), st->frameSize>>1);
         ol1 *= ol1*(st->frameSize>>1);
         ol2 *= ol2*(st->frameSize>>1);

         ol_gain2=ol1;
         if (ol2>ol1)
            ol_gain2=ol2;
         ol_gain2 = sqrt(2*ol_gain2*(ol1+ol2))*1.3*(1-.5*GAIN_SCALING_1*GAIN_SCALING_1*ol_pitch_coef*ol_pitch_coef);
      
         ol_gain=SHR(sqrt(1+ol_gain2/st->frameSize),SIG_SHIFT);

      } else {
#endif
         ol_gain = SHL32(EXTEND32(compute_rms(st->exc, st->frameSize)),SIG_SHIFT);
#ifdef EPIC_48K
      }
#endif
   }

   /*VBR stuff*/
   if (st->vbr && (st->vbr_enabled||st->vad_enabled))
   {
      float lsp_dist=0;
      for (i=0;i<st->lpcSize;i++)
         lsp_dist += (st->old_lsp[i] - st->lsp[i])*(st->old_lsp[i] - st->lsp[i]);
      lsp_dist /= LSP_SCALING*LSP_SCALING;
      
      if (st->abr_enabled)
      {
         float qual_change=0;
         if (st->abr_drift2 * st->abr_drift > 0)
         {
            /* Only adapt if long-term and short-term drift are the same sign */
            qual_change = -.00001*st->abr_drift/(1+st->abr_count);
            if (qual_change>.05)
               qual_change=.05;
            if (qual_change<-.05)
               qual_change=-.05;
         }
         st->vbr_quality += qual_change;
         if (st->vbr_quality>10)
            st->vbr_quality=10;
         if (st->vbr_quality<0)
            st->vbr_quality=0;
      }

      st->relative_quality = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, GAIN_SCALING_1*ol_pitch_coef);
      /*if (delta_qual<0)*/
      /*  delta_qual*=.1*(3+st->vbr_quality);*/
      if (st->vbr_enabled) 
      {
         int mode;
         int choice=0;
         float min_diff=100;
         mode = 8;
         while (mode)
         {
            int v1;
            float thresh;
            v1=(int)floor(st->vbr_quality);
            if (v1==10)
               thresh = vbr_nb_thresh[mode][v1];
            else
               thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
            if (st->relative_quality > thresh && 
                st->relative_quality-thresh<min_diff)
            {
               choice = mode;
               min_diff = st->relative_quality-thresh;
            }
            mode--;
         }
         mode=choice;
         if (mode==0)
         {
            if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
            {
               mode=1;
               st->dtx_count=1;
            } else {
               mode=0;
               st->dtx_count++;
            }
         } else {
            st->dtx_count=0;
         }

         speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);

         if (st->abr_enabled)
         {
            int bitrate;
            speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
            st->abr_drift+=(bitrate-st->abr_enabled);
            st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
            st->abr_count += 1.0;
         }

      } else {
         /*VAD only case*/
         int mode;
         if (st->relative_quality<2)
         {
            if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
            {
               st->dtx_count=1;
               mode=1;
            } else {
               mode=0;
               st->dtx_count++;
            }
         } else {
            st->dtx_count = 0;
            mode=st->submodeSelect;
         }
         /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
         st->submodeID=mode;
      } 
   } else {
      st->relative_quality = -1;
   }