/* 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.*/#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_bits.h"#include "vbr.h"#include "misc.h"#include "speex_callbacks.h"#ifdef SLOW_TRIG#include "math_approx.h"#define cos speex_cos#endif#ifndef M_PI#define M_PI           3.14159265358979323846  /* pi */#endif#ifndef NULL#define NULL 0#endif#define SUBMODE(x) st->submodes[st->submodeID]->xfloat exc_gain_quant_scal3[8]={-2.794750, -1.810660, -1.169850, -0.848119, -0.587190, -0.329818, -0.063266, 0.282826};float exc_gain_quant_scal1[2]={-0.35, 0.05};#define sqr(x) ((x)*(x))void *nb_encoder_init(SpeexMode *m){   EncState *st;   SpeexNBMode *mode;   int i;   mode=(SpeexNBMode *)m->mode;   st = (EncState*)speex_alloc(sizeof(EncState)+8000*sizeof(float));   if (!st)      return NULL;   st->stack = ((char*)st) + sizeof(EncState);      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->bufSize = mode->bufSize;   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->preemph = mode->preemph;     st->submodes=mode->submodes;   st->submodeID=st->submodeSelect=mode->defaultSubmode;   st->pre_mem=0;   st->pre_mem2=0;   st->bounded_pitch = 1;   /* Allocating input buffer */   st->inBuf = PUSH(st->stack, st->bufSize, float);   st->frame = st->inBuf + st->bufSize - st->windowSize;   /* Allocating excitation buffer */   st->excBuf = PUSH(st->stack, st->bufSize, float);   st->exc = st->excBuf + st->bufSize - st->windowSize;   st->swBuf = PUSH(st->stack, st->bufSize, float);   st->sw = st->swBuf + st->bufSize - st->windowSize;   st->exc2Buf = PUSH(st->stack, st->bufSize, float);   st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;   st->innov = PUSH(st->stack, st->frameSize, float);   /* Asymmetric "pseudo-Hamming" window */   {      int part1, part2;      part1 = st->subframeSize*7/2;      part2 = st->subframeSize*5/2;      st->window = PUSH(st->stack, st->windowSize, float);      for (i=0;i<part1;i++)         st->window[i]=.54-.46*cos(M_PI*i/part1);      for (i=0;i<part2;i++)         st->window[part1+i]=.54+.46*cos(M_PI*i/part2);   }   /* Create the window for autocorrelation (lag-windowing) */   st->lagWindow = PUSH(st->stack, st->lpcSize+1, float);   for (i=0;i<st->lpcSize+1;i++)      st->lagWindow[i]=exp(-.5*sqr(2*M_PI*st->lag_factor*i));   st->autocorr = PUSH(st->stack, st->lpcSize+1, float);   st->buf2 = PUSH(st->stack, st->windowSize, float);   st->lpc = PUSH(st->stack, st->lpcSize+1, float);   st->interp_lpc = PUSH(st->stack, st->lpcSize+1, float);   st->interp_qlpc = PUSH(st->stack, st->lpcSize+1, float);   st->bw_lpc1 = PUSH(st->stack, st->lpcSize+1, float);   st->bw_lpc2 = PUSH(st->stack, st->lpcSize+1, float);   st->lsp = PUSH(st->stack, st->lpcSize, float);   st->qlsp = PUSH(st->stack, st->lpcSize, float);   st->old_lsp = PUSH(st->stack, st->lpcSize, float);   st->old_qlsp = PUSH(st->stack, st->lpcSize, float);   st->interp_lsp = PUSH(st->stack, st->lpcSize, float);   st->interp_qlsp = PUSH(st->stack, st->lpcSize, float);   st->rc = PUSH(st->stack, st->lpcSize, float);   st->first = 1;   for (i=0;i<st->lpcSize;i++)   {      st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);   }   st->mem_sp = PUSH(st->stack, st->lpcSize, float);   st->mem_sw = PUSH(st->stack, st->lpcSize, float);   st->mem_sw_whole = PUSH(st->stack, st->lpcSize, float);   st->mem_exc = PUSH(st->stack, st->lpcSize, float);   st->pi_gain = PUSH(st->stack, st->nbSubframes, float);   st->pitch = PUSH(st->stack, st->nbSubframes, int);   st->vbr = PUSHS(st->stack, 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->complexity=2;   st->sampling_rate=8000;   st->dtx_count=0;   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, float *in, SpeexBits *bits){   EncState *st;   int i, sub, roots;   int ol_pitch;   float ol_pitch_coef;   float ol_gain;   float *res, *target, *mem;   char *stack;   float *syn_resp;   float lsp_dist=0;   float *orig;   st=(EncState *)state;   stack=st->stack;   /* Copy new data in input buffer */   speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));   st->inBuf[st->bufSize-st->frameSize] = in[0] - st->preemph*st->pre_mem;   for (i=1;i<st->frameSize;i++)      st->inBuf[st->bufSize-st->frameSize+i] = in[i] - st->preemph*in[i-1];   st->pre_mem = in[st->frameSize-1];   /* Move signals 1 frame towards the past */   speex_move(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));   speex_move(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));   speex_move(st->swBuf, st->swBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));   /* Window for analysis */   for (i=0;i<st->windowSize;i++)      st->buf2[i] = st->frame[i] * st->window[i];   /* Compute auto-correlation */   _spx_autocorr(st->buf2, st->autocorr, st->lpcSize+1, st->windowSize);   st->autocorr[0] += 10;        /* prevents NANs */   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] *= st->lagWindow[i];   /* Levinson-Durbin */   wld(st->lpc+1, st->autocorr, st->rc, st->lpcSize);   st->lpc[0]=1;   /* LPC to LSPs (x-domain) transform */   roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, 0.2, stack);   /* Check if we found all the roots */   if (roots==st->lpcSize)   {      /* LSP x-domain to angle domain*/      for (i=0;i<st->lpcSize;i++)         st->lsp[i] = acos(st->lsp[i]);   } else {      /* Search again if we can afford it */      if (st->complexity>1)         roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, 0.05, stack);      if (roots==st->lpcSize)       {         /* LSP x-domain to angle domain*/         for (i=0;i<st->lpcSize;i++)            st->lsp[i] = acos(st->lsp[i]);      } else {         /*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];         }      }   }   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]);   /* 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         for (i=0;i<st->lpcSize;i++)            st->interp_lsp[i] = .375*st->old_lsp[i] + .625*st->lsp[i];      lsp_enforce_margin(st->interp_lsp, st->lpcSize, .002);      /* Compute interpolated LPCs (unquantized) for whole frame*/      for (i=0;i<st->lpcSize;i++)         st->interp_lsp[i] = cos(st->interp_lsp[i]);      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];         float 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++)         {            if ((nol_pitch_coef[i]>.85*ol_pitch_coef) &&                 (fabs(nol_pitch[i]-ol_pitch/2.0)<=1 || fabs(nol_pitch[i]-ol_pitch/3.0)<=1 ||                  fabs(nol_pitch[i]-ol_pitch/4.0)<=1 || fabs(nol_pitch[i]-ol_pitch/5.0)<=1))            {               /*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);*/      } 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 */      ol_gain=0;      for (i=0;i<st->frameSize;i++)         ol_gain += st->exc[i]*st->exc[i];            ol_gain=sqrt(1+ol_gain/st->frameSize);   }   /*VBR stuff*/   if (st->vbr && (st->vbr_enabled||st->vad_enabled))   {            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, 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;   }