#include "common.h"
#include <math.h>

#ifdef ENX11
disp_data dispdata;
disp_data *disdp=&dispdata;
#endif

enum {LOWPASS,BANDPASS,HIGHPASS};

extern fsk_data *fskdp;

/**********************************************************
IIR Biquad Filters.
**********************************************************/

extern inline float biq_lp(float x,float *pcoef,float *buf)
{
	float y;
	y =(*pcoef++)*(x+buf[0]+buf[0]+buf[1]);
	y+=(*pcoef++)*buf[2];
	y+=(*pcoef)*buf[3];

	buf[1]=buf[0]; buf[0]=x;
	buf[3]=buf[2]; buf[2]=y;

	return y;
}

extern inline float biq_bp(float x,float *pcoef,float *buf)
{
	float y;
	y =(*pcoef++)*(x-buf[1]);
	y+=(*pcoef++)*buf[2];
	y+=(*pcoef)*buf[3];

	buf[1]=buf[0]; buf[0]=x;
	buf[3]=buf[2]; buf[2]=y;

	return y;
}

extern inline float biq_hp(float x,float *pcoef,float *buf)
{
	float y;
	y =(*pcoef++)*(x-buf[0]-buf[0]+buf[1]);
	y+=(*pcoef++)*buf[2];
	y+=(*pcoef)*buf[3];

	buf[1]=buf[0]; buf[0]=x;
	buf[3]=buf[2]; buf[2]=y;

	return y;
}

/*******************************************************
Coef. Calculation
*******************************************************/
void gen_coef(int tipo, float f0, float Q, float *pcoef)
{
	float w0,a,d;

	w0=2.*M_PI*f0;

	//Prewharping
	w0=2.0*FS*tan(w0/FS/2.0);

	a=FS/w0;
	d=4.*a*a +2.*a/Q +1.;

	switch(tipo){
	case LOWPASS:  (*pcoef++)=1.0/d;	break;
	case BANDPASS: (*pcoef++)=2.*a/Q/d;	break;
	case HIGHPASS: (*pcoef++)=4.*a*a/d;	break;
	}

	(*pcoef++)=(8.*a*a -2.)/d;
	(*pcoef++)=-(4.*a*a -2.*a/Q +1.)/d;
}

/*******************************************************
Decoder Variables & Coef. calculation
*******************************************************/
float lp1_c[3],lp2_c[3],bp0_c[3],bp1_c[3];
float lp1_b[4],lp2_b[4],bp0_b[4],bp0_b1[4],bp1_b[4],bp1_b1[4];

void set_filters(float f0, float f1, float dr)
{
	int i;

	gen_coef(BANDPASS,f0,f0/dr/2.0,bp0_c);	/* Space filter */
	gen_coef(BANDPASS,f1,f1/dr/2.0,bp1_c);	/* Mark filter  */
	gen_coef(LOWPASS,dr,0.5412,lp1_c);	/* Low-Pass order-4 Butt. filter */
	gen_coef(LOWPASS,dr,1.3066,lp2_c);

	//for (i=0;i<4;i++) lp1_b[i]=lp2_b[i]=bp0_b[i]=bp1_b[i]=0.0;
}

/*******************************************************
FSK & CW demodulation
*******************************************************/
float demodulator()
{
	float sample,xs,xm,y,ym;
        static float ymax=0.0,ymin=0.0;
	sample=get_audio_sample();

        if(fskdp->mode==RTTY || fskdp->mode==HDLC) { //FSK
	    xs=biq_bp(biq_bp(sample,bp0_c,bp0_b),bp0_c,bp0_b1);
            //xs=biq_bp(sample,bp0_c,bp0_b);
	    xm=biq_bp(biq_bp(sample,bp1_c,bp1_b),bp1_c,bp1_b1);
            //xm=biq_bp(sample,bp1_c,bp1_b);
	    xs*=xs;		/* xs RMS */
	    xm*=xm;		/* xm RMS */
	    y=biq_lp(biq_lp(xm-xs,lp1_c,lp1_b),lp2_c,lp2_b);
        }else{  //CW
            xm=biq_bp(biq_bp(sample,bp1_c,bp1_b),bp1_c,bp1_b1);
            xm*=xm;
            y=biq_lp(biq_lp(xm,lp1_c,lp1_b),lp2_c,lp2_b);
        }
#ifdef ENX11
        disdp->inp++;
	disdp->inp&=NDBUF-1;
	disdp->in[disdp->inp]=sample;
	disdp->outp++;
	disdp->outp&=NDBUF-1;
        disdp->out[disdp->outp]=y;
#endif
        return y;
}
