csub.c

HawkVoiceDI Project created and maintained by Phil Frisbie, Jr. <[e-mail]> HawkVoiceD

/*
  CELP voice codec, part of the HawkVoice Direct Interface (HVDI)
  cross platform network voice library
  Copyright (C) 2001-2003 Phil Frisbie, Jr. ([e-mail])

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Library General Public
  License as published by the Free Software Foundation; either
  version 2 of the License, or (at your option) any later version.
  
  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Library General Public License for more details.
    
  You should have received a copy of the GNU Library General Public
  License along with this library; if not, write to the
  Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  Boston, MA  02111-1307, USA.
      
  Or go to http://www.gnu.org/copyleft/lgpl.html
*/

#include <memory.h>
#include "celpint.h"
#include "ftol.h"

extern float x[MAXCODE];

extern void psearch(int l, int *pindex, celp_encoder_state *st, int *tauptr, int *minptr, float *d1b);
extern void pitchvql(float *rar, int idim, float *buf, int idimb, float *b);
extern void polefilt10(float *a, float *z, float *xy, int len);
extern void polefilt3(float *a, float *z, float *xy, int len);
extern void zerofilt10(float *b, float *z, float *xy, int len);
extern float gainencode(float input, int *index);

static void bwexp10(float alpha, float *pc, float *pcexp)
{
    float temp = 1.0f;

    pcexp[0] = pc[0];
    temp *= alpha;

    pcexp[1] = pc[1] * temp;
    temp *= alpha;

    pcexp[2] = pc[2] * temp;
    temp *= alpha;

    pcexp[3] = pc[3] * temp;
    temp *= alpha;

    pcexp[4] = pc[4] * temp;
    temp *= alpha;

    pcexp[5] = pc[5] * temp;
    temp *= alpha;

    pcexp[6] = pc[6] * temp;
    temp *= alpha;

    pcexp[7] = pc[7] * temp;
    temp *= alpha;

    pcexp[8] = pc[8] * temp;
    temp *= alpha;

    pcexp[9] = pc[9] * temp;
}

static float fastcgain(float *ex, int l, int first, int len, float *match, celp_encoder_state *st)
{
    register float cor;
    float cgain;
    int i, j, minlen;
    float *y = st->y;
    float *h = st->h;
    float *pex, *py;
    float eng = st->eng;
    
    if (first)
    {
        pex = ex;
        memset(y, 0, sizeof(y[0]) * l);
        for (i = 0; i < l; i++,pex++)
        {
            if (*pex > 0.0f)
            {
                py = &y[i];
                minlen = mmin(l-i, len);
                for(j = 0; j < minlen; j++)
                    py[j] += h[j];
            }
            else if (*pex < 0.0f)
            {
                py = &y[i];
                minlen = mmin(l-i, len);
                for(j = 0; j < minlen; j++)
                    py[j] -= h[j];
            }
        }
    }
    else
    {
        /* *First and second shift combined			 */
        for (i = l - 2; i > len + 1; i-=2)
        {
            y[i] = y[i-2];
        }
        y[len] = y[len - 2] + ex[1] * h[len - 1];
        /* *ternary stochastic code book (-1, 0, +1)			 */
        if (ex[1] > 0.0f)
        {
            if (ex[0] > 0.0f)
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] + h[i-1] + h[i];
            }
            else if(ex[0] < 0.0f)
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] + h[i-1] - h[i];
            }
            else
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] + h[i-1];
            }
        }
        else if (ex[1] < 0.0f)
        {
            if (ex[0] > 0.0f)
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] - h[i-1] + h[i];
            }
            else if(ex[0] < 0.0f)
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] - h[i-1] - h[i];
            }
            else
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] - h[i-1];
            }
        }
        else
        {
            if (ex[0] > 0.0f)
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] + h[i];
            }
            else if(ex[0] < 0.0f)
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2] - h[i];
            }
            else
            {
                for (i = len - 2; i > 1; i-=2)
                    y[i] = y[i-2];
            }
        }
        y[0] = ex[0] * h[0];
    }
    
    /**	Calculate correlation and energy:
    e0 = spectrum & pitch prediction residual
    y  = error weighting filtered code words
    
      \/\/\/  CELP's computations are focused in this correlation \/\/\/
      - For a 512 code book this correlation takes 4 MIPS!
    - Decimation?, Down-sample & decimate?, FEC codes?	*/
    
    cor = 0.0f;
    
    /* *End correct energy on subsequent code words:			 */
    if (lrintf(ex[0]) == 0 && lrintf(ex[1]) == 0 && !first)
    {
        py = y;
        for (i = 0; i < l; i+=2)
        {
            cor += y[i] * st->e0[i];
        }
        cor *= 2.0f;
        eng = eng - 2 * st->y59save * st->y59save;
    }
    else
    {
        py = y;
        eng = 0.0f;
        for (i = 0; i < l; i+=2)
        {
            float temp = py[i];

            eng += temp * temp;
            cor += temp * st->e0[i];
        }
        cor *= 2.0f;
        eng *= 2.0f;
    }
    st->y59save = y[l - 2];
    st->eng = eng;
    
    /*	Independent (open-loop) quantization of gain and match (index):	 */
    
    if (st->eng <= 0.0f)
        st->eng = 1.0f;
    cgain = cor / st->eng;
    *match = cor * cgain;
    
    return (cgain);
}

static float cgain(float *ex, int l, int first, int len, float *match, celp_encoder_state *st)
{
    register float cor;
    float cgain;
    int i, j, minlen;
    float *y = st->y;
    float *h = st->h;
    float *pex, *py;
    float eng = st->eng;
    
    if (first)
    {
        pex = ex;
        memset(y, 0, sizeof(y[0]) * l);
        for (i = 0; i < l; i++,pex++)
        {
            if (*pex > 0.0f)
            {
                py = &y[i];
                minlen = mmin(l-i, len);
                for(j = 0; j < minlen; j++)
                    py[j] += h[j];
            }
            else if (*pex < 0.0f)
            {
                py = &y[i];
                minlen = mmin(l-i, len);
                for(j = 0; j < minlen; j++)
                    py[j] -= h[j];
            }
        }
    }
    else
    {
        /* *First and second shift combined			 */

        for (i = l - 1; i > len + 1; i--)
        {
            y[i] = y[i-2];
        }
        y[len + 1] = y[len - 1];
        y[len] = y[len - 2] + ex[1] * h[len - 1];
        /* *ternary stochastic code book (-1, 0, +1)			 */
        if (ex[1] > 0.0f)
        {
            if (ex[0] > 0.0f)
            {
                for (i = len - 1; i > 1; i--)
                    y[i] = y[i-2] + h[i-1] + h[i];
            }
            else if(ex[0] < 0.0f)
            {
                for (i = len - 1; i > 1; i--)
                    y[i] = y[i-2] + h[i-1] - h[i];
            }
            else
            {
                for (i = len - 1; i > 1; i--)
                    y[i] = y[i-2] + h[i-1];
            }
        }
        else if (ex[1] < 0.0f)
        {
            if (ex[0] > 0.0f)
            {
                for (i = len - 1; i > 1; i--)
                    y[i] = y[i-2] - h[i-1] + h[i];
            }
            else if(ex[0] < 0.0f)
            {