fcoher.c

speech signal process tools

/*	

 This material contains proprietary software of Entropic Processing, Inc.
 Any reproduction, distribution, or publication without the the prior	   
 written permission of Entropic Processing, Inc. is strictly prohibited.
 Any public distribution of copies of this work authorized in writing by
 Entropic Processing, Inc. must bear the notice			
     
	Copyright 1986, Entropic Proccessing, Inc
	(C) 1985, Entropic Processing, Inc.
*/

#ifdef SCCS
static char *sccsid = "@(#)fcoher.c	1.4 8/20/86 EPI";
#endif

#define maxsze 16
#define mszsqr 1024
#define yes 1

#include <stdio.h>
#include <sps/sps.h>
#include <sps/pitch.h>

extern int  debug_level;
extern short    coh_method;
extern double    bne, bne_thr; /* Background Noise Energy */
double  sqrt (), Sout[mszsqr], iSout[mszsqr];
extern  FILE * hptr;
fast_coher (x, n, order, pitch, fast_update_flag, lookback_flag,
    pdet, pikdst, pcrosscor)
float   x[], *pdet, *pikdst, *pcrosscor;
int     n, order, pitch, fast_update_flag, lookback_flag;
{
    double  S[mszsqr], iS[mszsqr], iR[4 * maxsze];
    static double   R[4 * maxsze];
    double  pdist, Fg[2][2];
    double  Prc[maxsze], Qrc[maxsze];
    double  th_init[maxsze], Aw[maxsze], Ax[maxsze], eps = 1.0e-10;
    double  det, Rww[2 * maxsze], Rxx[2 * maxsze];
    float   t;
    float   tmp[maxsze], a[maxsze], b[maxsze], c[maxsze], d[maxsze];
    float   dd[2 * maxsze], dc[2 * maxsze], cd[2 * maxsze], cc[2 * maxsze];
    float   aa[2 * maxsze], ab[2 * maxsze], ba[2 * maxsze], bb[2 * maxsze];
    float   A[maxsze][2][2];
    float   ikd, optgain;
    double  t0, t1, t2;
    int     return_flag;
    int     init_siz = 0;
    int     i, j, k, matsiz;
    int     gmon_flag = (debug_level > 3);

    matsiz = order + 1;
    if (coh_method == STR2_COH && order > 0)
    {
	scovar (x, order, n, pitch, S);

	genburg (S, iS, &matsiz, &pdist, Sout, iSout, gmon_flag,
		&return_flag, R, iR);

	/* If Dan's program returned identity matrix, then return */

	if (Sout[0] == 1.0 && Sout[1] == 0.0)
	    return;

	for (i = 0; i < matsiz; i++)
	    for (j = 0; j < 2; j++)
		for (k = 0; k < 2; k++)
		    R[4 * i + 2 * j + k]
			= Sout[2 * i + 2 * matsiz * j + k];
    }
    else
	fast_cross (x, n, order, pitch, R, fast_update_flag, lookback_flag);

/*
    printf("pitch = %d\tnsmp = %d\tx[0] = %f\tR:%g\t%g\t%g\n",
		pitch, n, x[0], R[0], R[1], R[3]);
*/
    t = R[0] * R[3];
/*  Check if one of the channels is a sequence of zeros */
    if (t == 0.0)
    {
	*pdet = 1.0;
	*pcrosscor = 0.0;
	*pikdst = 1.0;
	return;
    }
    *pdet = 1.0 - R[1] * R[1] / t;
    *pcrosscor = R[1] / sqrt ((double) t);
    t = R[0] / R[3];
    *pikdst = t + 1.0 / t - 1;

/* Adaptive noise cancellation */
    if (R[0] > bne_thr * n && R[3] > bne_thr * n)
    {
	t0 = R[0] - n * bne;
	t1 = R[3] - n * bne;
	t2 = R[1] * R[1] / (t0 * t1);
	if (R[1] > 0.0 && t2 < 1.0 && order == 0)
	{
	    t = t0 * t1;
	    *pdet = 1.0 - t2;
	    *pcrosscor = R[1] / sqrt ((double) t);
	    t = t0 / t1;
	    *pikdst = t + 1.0 / t - 1;
	}
    }
    if (order == 0 || R[1] < 0)
	return;

    /* Note: mchlev treats R as a 3-d array */
    mchlev (R, order, A, Fg);
    for (i = 0; i < matsiz; i++)
    {
	a[i] = A[i][0][0];
	b[i] = A[i][0][1];
	c[i] = A[i][1][0];
	d[i] = A[i][1][1];
    }
    if (order == 1)		/* speed-up first order case */
    {
	aa[0] = aa[2] = a[1];
	aa[1] = 1.0 + a[1] * a[1];
	ba[0] = 0.0;
	ba[1] = b[1] * a[1];
	ba[2] = b[1];
	ab[0] = b[1];
	ab[1] = ba[1];
	ab[2] = 0.0;
	bb[0] = bb[2] = 0.0;
	bb[1] = b[1] * b[1];

	dd[0] = dd[2] = d[1];
	dd[1] = 1.0 + d[1] * d[1];
	cd[0] = 0.0;
	cd[1] = c[1] * d[1];
	cd[2] = c[1];
	dc[0] = c[1];
	dc[1] = cd[1];
	dc[2] = 0.0;
	cc[0] = cc[2] = 0.0;
	cc[1] = c[1] * c[1];
    }
    else
    {
	for (i = 0; i < matsiz; i++)
	    tmp[order - i] = a[i];
	convolve (a, matsiz, tmp, matsiz, aa);
	convolve (b, matsiz, tmp, matsiz, ba);

	for (i = 0; i < matsiz; i++)
	    tmp[order - i] = b[i];
	convolve (a, matsiz, tmp, matsiz, ab);
	convolve (b, matsiz, tmp, matsiz, bb);

	for (i = 0; i < matsiz; i++)
	    tmp[order - i] = c[i];
	convolve (c, matsiz, tmp, matsiz, cc);
	convolve (d, matsiz, tmp, matsiz, dc);

	for (i = 0; i < matsiz; i++)
	    tmp[order - i] = d[i];
	convolve (c, matsiz, tmp, matsiz, cd);
	convolve (d, matsiz, tmp, matsiz, dd);
    }

    for (i = 0; i < order + matsiz; i++)
    {
	Rxx[i] = Fg[0][0] * dd[i] - Fg[0][1] * cd[i]
	    - Fg[1][0] * dc[i] + Fg[1][1] * cc[i];
	Rww[i] = Fg[0][0] * bb[i] - Fg[0][1] * ab[i]
	    - Fg[1][0] * ba[i] + Fg[1][1] * aa[i];
    }

    det = Fg[0][0] * Fg[1][1] - Fg[0][1] * Fg[1][0];

    wilson (&Rww[order], matsiz, th_init, init_siz, Aw, Qrc, eps);
    Qrc[0] = Aw[0] * Aw[0];
    if (debug_level > 2)
	fprintf (stderr,
		"pitch = %d\tdet = %f   G1 = %f\n", pitch, det, Qrc[0]);
    det = det / Qrc[0];

    wilson (&Rxx[order], matsiz, th_init, init_siz, Ax, Prc, eps);
    Prc[0] = Ax[0] * Ax[0];
    det = det / Prc[0];

    if (debug_level > 2)
	fprintf (stderr, "G2 = %f   det  = %f\n", Prc[0], det);
    if (det < 0.0 && hptr)
	fprintf (hptr, "det = %g\n", det);
    IKdistn (Prc, Qrc, order, &ikd, &optgain);
/*    *pikdst = ikd; */
    *pdet = det;
}

fast_cross (x, n, order, pitch, R, fast_update_flag, lookback_flag)
float   x[];
double  R[];
int     n, order, pitch, fast_update_flag, lookback_flag;
{
    static int  prev_n = 0;
    double  temp;
    float  *ptr1, *ptr2;
    int     i, j, k, l;
#define reg_prev_n	j
    n += order;
    if (fast_update_flag)
    {
	j = prev_n;
	if (lookback_flag)
	{

	    for (i = 0; i <= order; i++)
	    {
		/*  compute auto of trace 1 */
		temp = x[0] * x[i];
		if (n == reg_prev_n)
		    temp -= x[n - i] * x[n];
		R[4 * i] += temp;

		/*  compute auto of trace 2 */
		if (n > reg_prev_n)
		    R[4 * i + 3] += x[j + pitch - i] * x[j + pitch];
	    }

	    for (i = order; i > 0; i--)
	    {
		/*  compute R[pitch - 2], R[pitch - 1], etc. */
		temp = R[4 * i - 2];
		if (n == reg_prev_n)
		    temp -= x[pitch + n - i] * x[n];
		R[4 * i + 2] = temp;
	    }

	    for (i = 0; i < order; i++)
	    {
		/*  compute R[pitch + 1], R[pitch + 2], etc. */
		temp = R[4 * i + 5] + x[0] * x[pitch + i];
		if (n > reg_prev_n)
		    temp += x[j + pitch] * x[j - i];
		R[4 * i + 1] = temp;
	    }
	}
	else
	{

	    for (i = 0; i <= order; i++)
	    {
		/*  compute auto of trace 1 */
		if (n > reg_prev_n)
		    R[4 * i] += x[j] * x[j - i];

		/*  compute auto of trace 2 */
		temp = -x[pitch - 1] * x[pitch - 1 + i];
		ptr1 = &x[pitch + j - 1];
		ptr2 = &x[pitch + j - i - 1];
		for (l = reg_prev_n - 1; l < n; l++)
		    temp += *ptr1++ * *ptr2++;
		R[4 * i + 3] += temp;
	    }

	    for (i = order; i > 0; i--)
	    {
		/*  compute R[pitch - 2], R[pitch - 1], etc. */
		R[4 * i + 2] = R[4 * i - 2] - x[i - 1] * x[pitch - 1];
		if (n > reg_prev_n)
		    R[4 * i + 2] += x[j] * x[pitch + j - i];
	    }

	    for (i = 0; i < order; i++)
	    {
		/*  compute R[pitch + 1], R[pitch + 2], etc. */
		temp = R[4 * i + 5];
		ptr1 = &x[j - 1 - i];
		ptr2 = &x[pitch + j - 1];
		for (l = reg_prev_n - 1; l < n; l++)
		    temp += *ptr1++ * *ptr2++;
		R[4 * i + 1] = temp;
	    }

	}

	/*  compute R[pitch + order] */
	temp = 0.0;
	ptr1 = &x[0];
	ptr2 = &x[pitch + order];
	for (l = order; l < n; l++)
	    temp += *ptr1++ * *ptr2++;
	R[order * 4 + 1] = temp;
    }
    else
    {
	double *ptr3;

	for (j = 0; j < 2; j++)
	    for (k = j; k < 2; k++)
	    {
		temp = 0.0;
		ptr1 = &x[j * pitch];
		ptr2 = &x[k * pitch];
		for (l = 0; l < n; l++)
		    temp += *ptr1++ * *ptr2++;
		R[2 * j + k] = temp;
	    }
	ptr3 = &R[4];
	for (i = 1; i <= order; i++)
	{
	    for (j = 0; j <= pitch; j += pitch)
		for (k = 0; k <= pitch; k += pitch)
		{
		    temp = 0.0;
		    ptr1 = &x[j];
		    ptr2 = &x[k + i];
		    for (l = i; l < n; l++)
			temp += *ptr1++ * *ptr2++;
		    *ptr3++ = temp;
		}
	}
    }
    R[2] = R[1];
    prev_n = n;
}