/*****************************************************************
* Application: 	DTMF codec
* File:		dtmf.c
* Description:	Implementation of DTMF C-functions
*		
* History:
* Date		Who		Comment
* 08-20-97	Gunter Schmer	creation
* 04-17-98	Gunter Schmer	Increase freq resolution to 49Hz
*				Do preselect of DTMF freq with 98Hz resolution
*
*****************************************************************/
#if !defined(NO_RTA)
#include <std.h>
#include <log.h>
#endif

#include "dtmf.h"

static void dtmfNormalize(short, short *);
static int dtmfDigitDetect(DTMF_DecObj*, short *);

extern unsigned char dtmfCheck(unsigned char);
extern void dtmfMaxSearch(short*,short*);
extern unsigned char dtmfCheckRelPeak(short,short,unsigned char);
extern short sqrt(int);					/* .asm */
extern short div_q15(short, short);			/* .asm */
extern long dotprod40(short*, short*, int);		/* .asm */
extern void scale_q15(short*, short, int);		/* .asm */
extern void goertzel(short*, short*, int, int, short);	/* .asm */


/* -------------------- global variables -------------------------- */
short 	energy[12] = 	{0,0,0,0,0,0,0,0,0,0,0,0};  /* needs to be alligned */

int 	coef10 = 0x694C6D4C;
int 	coef32 = 0x5E9B6465;
int	coef54 = 0x3FC54A81;
int	coef76 = 0x2463331D;

short	coef1st[8] = {	0x6D4C,
			0x694C,
			0x6465,
			0x5E9B,
			0x4A81,
			0x3FC5,
			0x331D,
			0x2463	};

short	coef2nd[8] = {	0x3993,
			0x2C96,
			0x1D7D,
			0x0BD8,
			0xD6BB,
			0xBF89,
			0xA8D2,
			0x94B0	};

unsigned char 	keys[16] = {	0x01,
				0x02,
				0x03,
				0x0A,
				0x04,
				0x05,
				0x06,
				0x0B,
				0x07,
				0x08,
				0x09,
				0x0C,
				0x0E,
				0x00,
				0x0F,
				0x0D	};

short	thresSignalStrength = 7045;	/* Q15: 0.215  */
short	thresPauseStrength = 1500;	/* Q15: 0.0457 */
short	thresReverseTwist = 4125;	/* Q15: 0.126  */
short	thresStandardTwist = 8231;	/* Q15: 0.251  */
short	thresRelativePeak = 6553;	/* Q15: 0.200  */
short	thres2ndHarmonics = 6553;	/* Q15: 0.200  */

#ifdef VERBOSE_RTA	
#define VERBOSE_LOG_printf(l,s) LOG_printf(&trace, s)
#else
#define VERBOSE_LOG_printf(l,s) 
#endif


/* -------------------- DTMF functions -------------------------- */

int DTMF_decode(DTMF_DecObj *chxDecObjPtr, short *buffer)
{
    if (*buffer != 0) {
	foo();
    }
	
	/* perform normalization */
	dtmfNormalize(164, buffer);

	/* perform digit detection and validation */
	return dtmfDigitDetect(chxDecObjPtr, buffer);
}



static int dtmfDigitDetect(DTMF_DecObj *chxDecObjPtr, short *buffer)
{
    short rowMaxIndex, colMaxIndex;
    int   coef1, coef2;
    int digit = -1;
    int keysTableIndex;
    unsigned char flags;

    flags     = chxDecObjPtr->flags;

    /* perform Goertzel filters to obtain energy template */
    goertzel(buffer,&energy[0],coef10,coef32,82);
    goertzel(buffer,&energy[4],coef54,coef76,82);


    dtmfMaxSearch(	&rowMaxIndex,
			&colMaxIndex	);

    flags = flags & 0x83;			/* clear all error flags */

    /****************************************
     *** if waiting for signal or silence ***
     ****************************************/
    if (chxDecObjPtr->state == DTMF_SIG_WAIT)	{
	VERBOSE_LOG_printf(&trace, "Detector in SIG_WAIT ...");

	if((energy[rowMaxIndex]+energy[colMaxIndex])
	   > thresPauseStrength) {
	    chxDecObjPtr->state = DTMF_DIGIT_STATE; /* enable digit mode */
	    LOG_printf(&trace,
		       "... strength is adequate, going to DTMF_DIGIT_STATE");
	}
    } else  if (chxDecObjPtr->state == DTMF_PAUSE_WAIT)	{
	if((energy[rowMaxIndex]+energy[colMaxIndex])
	   < thresPauseStrength) {
	    chxDecObjPtr->state = DTMF_SIG_WAIT; /* Wait for next digit */
	    LOG_printf(&trace,
		       "... strength below threshold, going to DTMF_SIG_WAIT");
	}
    } 

    /*******************************
     *** if in digit mode ***
     *******************************/
    else	{
	VERBOSE_LOG_printf(&trace, "Detector in digit mode ...");
	if(!(flags & 0xFC))  
	    flags = dtmfCheckRelPeak(rowMaxIndex,colMaxIndex,flags);
	if(!(flags & 0xFC))	{
	    coef1 = (coef1st[colMaxIndex] << 16) + coef1st[rowMaxIndex];
	    coef2 = (coef2nd[colMaxIndex] << 16) + coef2nd[rowMaxIndex];
	    goertzel(&buffer[0],&energy[8],coef1,coef2,164);
	}
	if(!(flags & 0xFC))
	    flags = dtmfCheck(flags);  
	if(!(flags & 0xFC))	{  
	    keysTableIndex = (rowMaxIndex<<2) + (colMaxIndex-4);
	    digit = keys[keysTableIndex]; /* decoded digit	*/
	    chxDecObjPtr->state = DTMF_PAUSE_WAIT; /* wait for digit to end */
	}

	/* report on progress */
	if (digit != -1) {
	    LOG_printf(&trace, "... digit %x detected", digit);
	} else {
	    /* report on flags */
	    if (flags & DTMF_SIGSTRENGTH) {
		VERBOSE_LOG_printf(&trace, "... error, signal strength");
	    }
	    if (flags & DTMF_REVERSETWIST) {
		VERBOSE_LOG_printf(&trace, "... error, reverse twist");
	    }
	    if (flags & DTMF_TWIST) {
		VERBOSE_LOG_printf(&trace, "... error, standard twist");
	    }
	    if (flags & DTMF_RELPEAK) {
		VERBOSE_LOG_printf(&trace, "... error, relative peak");
	    }
	    if (flags & DTMF_2NDHARM) {
		VERBOSE_LOG_printf(&trace, "... error, 2nd harmonic");
	    }
	}
    }

    chxDecObjPtr->flags	= flags;

    return digit;
}



static void dtmfNormalize(short N, short *buffer)
{
	int 	normPower;		/* (Q30)			*/
	int 	normLimit=0x00018F9C;	/* = 1/(64*164) 0x00018F9C	(Q30)	*/
	short	normConstant=0x4FF6;	/* = sqrt(64/164) 0x4FF6	(Q15)	*/
	short	normLevel=0x008D;	/* = 0.5*sqrt(2)/164 0x008D	(Q15)	*/
	short	normAmplitude;
	short	normScale;

	normPower = (int)(dotprod40(buffer,buffer,N) >> 6);

	if(normPower>normLimit)		{
		normAmplitude = sqrt(normPower);
		normAmplitude = ((int)normAmplitude * (int)normConstant) >> 15;
		normScale = div_q15(normLevel,normAmplitude);
		scale_q15(buffer,normScale,N);
	}
}

