csub.c

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

                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[0] > 0.0f)
            {
                for (i = len - 1; i > 1; i--)
                    y[i] = y[i-2] + h[i];
            }
            else if(ex[0] < 0.0f)
            {
                for (i = len - 1; i > 1; i--)
                    y[i] = y[i-2] - h[i];
            }
            else
            {
                for (i = len - 1; i > 1; i--)
                    y[i] = y[i-2];
            }
        }
        y[1] = ex[1] * h[0] + ex[0] * h[1];
        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++, py++)
        {
            cor += *py * st->e0[i];
        }
        eng = eng - st->y59save * st->y59save - st->y60save * st->y60save;
    }
    else
    {
        py = y;
        eng = 0.0f;
        for (i = 0; i < l; i++)
        {
            float temp = py[i];

            eng += temp * temp;
            cor += temp * st->e0[i];
        }
    }
    st->y59save = y[l - 2];
    st->y60save = y[l - 1];
    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 void mexcite1(int l, celp_encoder_state *st)
{
    int i;
    
    /* *e1 = Euclidean norm of the first error signal		*/
    /* (note: the error signal array e0 is reused)		*/
    
    st->e1 = 1e-6f;
    for (i = 0; i < l; i++)
    {
        st->e0save[i] = st->e0[i];
        st->e1 += st->e0[i] * st->e0[i];
    }
}

static void mexcite2(int l, celp_encoder_state *st)
{
    int i;
    
    /* *ccor = crosscorrelation of the residual signals before 	*/
    /* *and after pitch prediction				*/
    /* *(note: the error signal array e0 is reused		*/
    
    st->ccor = 1e-6f;
    for (i = 0; i < l; i++)
        st->ccor += st->e0[i] * st->e0save[i];
    
    /* *normalize the crosscorrelation				*/
    
    st->ccor = st->ccor / st->e1;
}

static void mexcite3(float *cgain, celp_encoder_state *st)
{
    float scale;
    
    /* *square root crosscorrelation scaling			*/
    
    scale = (float)sqrt(fabs(st->ccor));
    
    /* *modify scale						*/
    
    if (scale < 0.2f)
        scale = 0.2f;
    else if (scale > 0.9f) 
        scale = scale * 1.4f;
    
    /* *modify the stochastic component				*/
    
    *cgain = *cgain * scale;
}

static void cbsearch(int l, float *v, int *cbindex, int *gindex, celp_encoder_state *st)
{
    int i, codeword, index, cblength;
    float emax, gain, gmax, err, *px, *pv;
    
    codeword = 2*MAXNCSIZE - 2;
    index = 1;
    cblength = st->cblength;
    if(st->fastgain == 1)
    {
        gain = fastcgain(&x[codeword], l, TRUE, st->inpulselen, &err, st);
        emax = err;
        gmax = gain;
        codeword -= 2;
        for (i = 1; i < cblength; i++)
        {
            gain = fastcgain(&x[codeword], l, FALSE, st->inpulselen, &err, st);
            codeword -= 2;
            if (err >= emax)
            {
                gmax = gain;
                emax = err;
                index = i + 1;
            }
        }
    }
    else
    {
        gain = cgain(&x[codeword], l, TRUE, st->inpulselen, &err, st);
        emax = err;
        gmax = gain;
        codeword -= 2;
        for (i = 1; i < cblength; i++)
        {
            gain = cgain(&x[codeword], l, FALSE, st->inpulselen, &err, st);
            codeword -= 2;
            if (err >= emax)
            {
                gmax = gain;
                emax = err;
                index = i + 1;
            }
        }
    }
    
    /*		*pointer to best code word				*/
    
    codeword = 2*(MAXNCSIZE - index);
    
    /*		*OPTIONAL (may be useful for integer DSPs)		*/
    /*		*given best code word, recompute its gain to		*/
    /*		*correct any accumulated errors in recursions		*/
    /*  gain[*cbindex-1] = cgain(&x[codeword], l, TRUE, l, &err[*cbindex-1], st); */
    
    /* *constrained excitation						*/
    mexcite3(&gmax, st);
    
    /*		*gain quantization, UNNECESSARY for closed-loop quant	*/
    
    gmax = gainencode(gmax, gindex);
    
    /*		*scale selected code word vector -> excitation array	*/
    /*		*call VDECODE?						*/
    px = x + codeword;
    pv = v;
    for (i = 0; i < l; i++, pv++, px++)
        *pv = gmax * *px;
    *cbindex = index;
}

static void impulse(int l, float *fce, celp_encoder_state *st)
{
    float d5[MAXNO+1];

    memset(st->h, 0, sizeof(st->h[0]) * l);
    memset(d5, 0, sizeof(d5[0]) * (MAXNO+1));
    st->h[0] = 1.0;

    polefilt10(fce, d5, st->h, l);
}

static void movefr(int n, float *a, float *b)
{
    int i;
    
    for (i = 0; i < n; i++)
        *b++ = *a++;
}


static void confg(float *s, int l, float *d1, float *d2, float *d3, float *d4, int isw1,
                  float *fci, float *fce, celp_encoder_state *st)
{
    int i;

    if (isw1 != 0)
        pitchvql(st->e0, l, d1, st->idb, st->bb);
    polefilt10(fci, d2, st->e0, l);
    
    for (i = 0; i < l; i++)
        st->e0[i] = s[i] - st->e0[i];
    
    zerofilt10(fci, d3, st->e0, l);
    polefilt10(fce, d4, st->e0, l);
}

void csub(float *s, float *v, int l, int *cbindex, int *gindex, celp_encoder_state *st,
          float *fci, int *pindex, int *tauptr, int *minptr)
{
    float fce[MAXNO+1];

    memset(fce, 0, sizeof(fce));
    bwexp10(st->gamma2, fci, fce);

    /* *find the intial error without pitch VQ		 	 */

    memset(st->e0, 0, sizeof(st->e0[0]) * l);
    confg(s, l, st->d1a, st->d2a, st->d3a, st->d4a, 0, fci, fce, st);
    movefr(MAXNO + 1, st->d2b, st->d2a);
    movefr(MAXNO + 1, st->d3b, st->d3a);
    movefr(MAXNO + 1, st->d4b, st->d4a);
    
    /* *find impulse response (h) of perceptual weighting filter	 */
    
    impulse(l, fce, st);
    
    /* *norm of the first error signal for const. exc.		 */
    
    mexcite1(l, st);
    
    /* *pitch (adaptive code book) search						 */
    
    psearch(l, pindex, st, tauptr, minptr, st->d1b);
    
    /* *find initial error with pitch VQ
    */
    memset(st->e0, 0, sizeof(st->e0[0]) * l);
    confg(s, l, st->d1a, st->d2a, st->d3a, st->d4a, 1, fci, fce, st);
    
    /* *norm of second error signal for const. exc.		 */
    
    mexcite2(l, st);
    
    /* *stochastic code book search 				 */
    
    cbsearch(l, v, cbindex, gindex, st);
    
    /* *update filter states 					 */
    
    movefr(l, v, st->e0);
    confg(s, l, st->d1b, st->d2b, st->d3b, st->d4b, 1, fci, fce, st);
    movefr(st->idb, st->d1b, st->d1a);
    movefr(MAXNO + 1, st->d2b, st->d2a);
    movefr(MAXNO + 1, st->d3b, st->d3a);
    movefr(MAXNO + 1, st->d4b, st->d4a);
}