/*-------------------------------------------------------------------------*\

	FILE........: DECFR.C
	TYPE........: C Module
	COMPANY.....: VoiceTronix
	AUTHOR......: David Rowe
	DATE CREATED: 21/10/96

	Computationally intensive part of DTMF decoder function.

\*-------------------------------------------------------------------------*/

#include <stdio.h>
#include "decfr.h"
#include "dtmfcomm.h"
#include "dtmfcnst.h"


#define NFREQ		8	/* number of DTMF filters		*/


/* weights for 8 DTCF transform frequencies in 2.14 format */
/* 2cos(2*pi*f/Fs)* 2^14 */

#if 0
static Word16 dtmf_coeff[] = {
    27979,
    26956,
    25701,
    24218,
    19072,
    16324,
    13084,
    9314};
#endif

static double dtmf_coefff[] = {
    27979.0/(1<<14),
    26956.0/(1<<14),
    25701.0/(1<<14),
    24218.0/(1<<14),
    19072.0/(1<<14),
    16324.0/(1<<14),
    13084.0/(1<<14),
    9314.0/(1<<14)};

void dtmf_inner(short *x, double c, double *y0, double  *y1);


#ifndef WITH_ASM
// there is an assembler version of this function for pentium machines.
void dtmf_inner(short *x, double c, double *y0, double  *y1)
{
	float ymemf0 = 0;
	float ymemf1 = 0;
	float yf = 0;
	int   i;

	for(i=0; i<NDTMF; i++) {
	    yf = x[i] + c * ymemf0 - ymemf1;
	    ymemf1 = ymemf0;
	    ymemf0 = yf;
	}

	yf = c * ymemf0 - ymemf1;
	*y1 = ymemf1 = ymemf0;
	*y0 = ymemf0 = yf;
}
#endif

// hand optimising this function in assembler will save another
// 1% of CPU for a P200
static float energy(short x[])
{
	float E = 0.0;
	int   i = 0;

	for(; i < NDTMF/2; ++i, ++x) E += *x * *x;

	return E / (1<<(14+12));
}

/*---------------------------------------------------------------------------*\

	FUNCTION....: decode_frame()

	AUTHOR......: David Rowe
	DATE CREATED: 31/10/97

	Performs the filtering and energy calculation parts of the DTMF
	decoder.  This function has been modified from fixed point
	simulation originally designed for C5x implementation.  Although
	float arithmetic is used internally, the interfaces are in scaled
	fixed point to maintain compatiility with other modules.

	TO DO: Future work might be to convert all DTMF modules to float,
	which will speed things up a little.  Other possibilities include
	MMX or SIMD implementations.  

\*---------------------------------------------------------------------------*/

void decode_frame(short dtcf[], short *E1, short *E2, short x[])
/*  short  dtcf[];	filter outputs				*/
/*  short  *E1;		energy of first half of frame		*/
/*  short  *E2;		energy of second half of frame		*/
/*  short  x[];		input samples				*/
{
    double  ymemf0, ymemf1;
    double  yf;
    int     j = 0;

    /* Evaluate DTCF mag squared at each frequency -------------------------*/
    for(;j < NFREQ; ++j)
    {
	/*
	   Recursive part of filter:

	       y(n) = x(n) + c*y(n-1) - y(n-2)

	   Symbol		Variable		Format
	   ---------------------------------------------------
	   x(n)			st->x[i]		16.0
	   y(n)			acc			17.15
	   y(n-1),y(n-2)	ymem[0],ymem[1]		16.0
	   c			dtmf_coeff[j]            2.14

	   Note input scaling, input divided by 2^6, therefore
	   |X(fi)|^2 divided by 2^12.
	*/

	dtmf_inner(x, dtmf_coefff[j], &ymemf0, &ymemf1);

	yf = ymemf0*ymemf0 + ymemf1*ymemf1;
	yf -= dtmf_coefff[j] * ymemf0 * ymemf1;
	yf /= (float)(1<<(14+12));
	if(yf > 32767) yf = 32767;
	dtcf[j] = yf;
    }

    /* Determine energy of frame halves */

    *E1 = energy(x);
    *E2 = energy(&x[NDTMF/2]);
}

