#include "cmd_fir.h"

#define	pi	3.14159265

typedef struct filter_t
{
	double fs,fc1,fc2,beta;
	int n,n1;
	int wtype;
	int type;
	int nflag;
} filter_t;

#define FILT_LP   1
#define FILT_HP   2
#define FILT_BP   3
#define FILT_BS   4
#define FILT_CHECK(X) (((X)>=FILT_LP)&&((X)<=FILT_BS))

#define FILT_WIN_RECT 1
#define FILT_WIN_HAMM 2
#define FILT_WIN_HANN 3
#define FILT_WIN_BLCK 4
#define FILT_WIN_KAIS 5
#define FILT_WIN_CHECK(X) (((X)>=FILT_WIN_RECT)&&((X)<=FILT_WIN_KAIS))

void	sincx1_(double fc, double *sinc_h, int n1, int parity);
int     filter_spec_(filter_t *filt, int type, double fs, double fc1, double fc2, int wtype, double beta, int n);
void    lowpass_highpass_(filter_t *filt, double *sinc_h);
void	window_func_(filter_t *filt, double *win);
void	filter_output_(filter_t *filt, double *h, double *hb);
double	bessel(double x);

void filter_calc(filter_t *filt, double *hc, int hc_sz);



int filter_spec_(filter_t *filt, int type, double fs, double fc1, double fc2, int wtype, double beta, int n)
{

	/*Check input params*/
	//check sample rate
	if (fs < 0)
	{
		ERROR("ERROR fs\n");
		return -1;
	}
	filt->fs = fs;

	//check filter type
	if (!FILT_CHECK(type))
	{
		ENL();
		return -1;
	}
	filt->type = type;

	if ((filt->type==FILT_LP)||(filt->type==FILT_HP))
	{
		//PRINT("%f %f\n",fc1, fs);
		filt->fc1 = fc1/fs; //fc2 not used
	} else if ((type==FILT_BS)||(type==FILT_BP))
	{
		filt->fc1 = fc1/fs;
		filt->fc2 = fc2/fs;
		//PRINT("%f %f %f\n", fc1, fc2, fs);
	} else
	{
		ENL();
		return -1;
	}

	//window type used
	if (FILT_WIN_CHECK(wtype))
	{
		if (wtype == FILT_WIN_KAIS)
		{
			//beta for Kaiser window
			filt->beta = beta;
		}
	}
	filt->wtype = wtype;

	/* request filter length and determine whether it is odd or even
	* for N even only N/2 coefficients need be calculated, and for N odd
	* only (N+1)/2  because of the symmetry in h(n).
	*/

	filt->n = n;
	filt->n1 = n/2;
	filt->nflag = 1;
	if ((filt->n - 2*filt->n1)==1)
	{
		filt->nflag = 0;
		filt->n1 = filt->n1+1;
	}
	return 0;
}

//sinc_h size as filt->n1 size
void lowpass_highpass_(filter_t *filt, double *h)
{
	int j;

	sincx1_(filt->fc1, h, filt->n1, filt->nflag);
	if (filt->type == FILT_HP)
	{
		h[0]=1-h[0];
		for (j=0;j<filt->n1;++j)
		{
			h[j] = -h[j];
			//printf("%f ",sinc_h[j]);
		}
	}

}


void bandpass_bandstop_(filter_t *filt, double *sinc_h, double *h_bp)
{
	int j;

	double fc;

	fc = filt->fc2;
	sincx1_(fc, sinc_h, filt->n1, filt->nflag);
	for (j=0; j<filt->n1;++j)
	{
		h_bp[j] = sinc_h[j];
	}

	fc = filt->fc1;
	sincx1_(fc, sinc_h, filt->n1, filt->nflag);
	for (j=0;j<filt->n1;++j)
	{
		sinc_h[j] = h_bp[j] - sinc_h[j];
	}

	if (filt->type == FILT_BS)
	{
		sinc_h[0] = 1 - sinc_h[0];
		for (j=1; j<filt->n1; ++j)
		{
			sinc_h[j] = -sinc_h[j];
		}
	}
}

/*----------------------------------------------------------------------
* function to calculate sincx: 2FC * sin(nwc)/nwc
*/
void	sincx1_(double fc, double *h, int n1, int parity)
{
	int	i,j;
	double	omega, p;

	omega=2*pi*fc;
	h[0]=2*fc;
	i=1;
	if(parity==1)
	{	/* check for N even */
		i=0;
	}

	for(j=i; j<n1; j++)
	{
		p=parity*0.5+j;
		h[j]=2*fc*sin(omega*p)/(omega*p);
	}
}


void	window_func_(filter_t *filt, double *win)
{
	int	j;
	double	bd,p,rm,bn,x1;

	switch (filt->wtype)
	{
	case FILT_WIN_RECT:
		for (j=0; j<filt->n1; ++j)
		{
			win[j]=1.0;
		}
		break;
	case FILT_WIN_HAMM:
		for (j=0; j<filt->n1; ++j)
		{
			p=filt->nflag*0.5+j;
			win[j] = 0.54+(1-0.54)*cos(2*pi*p/filt->n);
		}
		break;
	case FILT_WIN_HANN:
		for (j=0; j<filt->n1; ++j)
		{
			p = filt->nflag*0.5 + j;
			win[j] = 0.5+(1-0.5)*cos(2*pi*p/filt->n);
		}
		break;
	case FILT_WIN_BLCK:
		for (j=0; j<filt->n1; ++j)
		{
			p = filt->nflag*0.5 + j;
			win[j] = 0.42 
				+ 0.5*cos(2*pi*p/(filt->n-1)) 
				+ 0.08*cos(4*pi*p/(filt->n-1));
		}
		break;
	case FILT_WIN_KAIS:
		x1 = filt->beta;
		bd = bessel(x1);
		for (j=0; j<filt->n1; ++j)
		{
			p = filt->nflag*0.5 + j;
			rm = 2*p/(filt->n);
			rm = rm*rm;
			x1 = filt->beta * sqrt(1-rm);
			bn = bessel(x1);
			win[j] = bn/bd;
		}
		break;
	default:
		printf("Unknown window type %d\n", filt->wtype);
	}
}

/*
*
*/
double	bessel(double x)
{
	double	y,t,t1,e,de,sde;
	int	i;

	y=x/2;
	t=1.E-08; e=1; de=1;
	for(i=1; i <25; ++i){
		t1=(double) i;
		de=de*y/t1;
		sde=de*de;
		e=e+sde;
		if((e*t-sde) > 0.0)
			return(e);
	}
	ENL();
	return 0.0;
}


void	filter_output_(filter_t *filt, double *h, double *hb)
{
	int j;

	double fs,fc,fc1,fc2;

	fs  = filt->fs;
	fc  = filt->fc1 * filt->fs;
	fc1 = filt->fc1 * filt->fs;
	fc2 = filt->fc2 * filt->fs;

	switch(filt->type)
	{
	case FILT_LP:	
		printf("lowpass filter \n");
		printf("sampling frequency:\t%f\n", fs);
		printf("cutoff frequency:\t%f\n", fc);
		break;
	case FILT_HP:	
		printf("highpass filter \n");
		printf("sampling frequency:\t%f\n", fs);
		printf("cutoff frequency:\t%f\n", fc);
		break;
	case FILT_BP:	
		printf("bandpass filter \n");
		printf("sampling frequency:\t%f\n", fs);
		printf("cutoff frequencies:\t%f\t%f\n", fc1, fc2);
		break;
	case FILT_BS:	
		printf("bandstop filter \n");
		printf("sampling frequency:\t%f\n", fs);
		printf("cutoff frequencies:\t%f\t%f\n", fc1, fc2);
		break;
	}
	printf("\n");

	switch(filt->wtype)
	{
	case 1:	
		printf("Rectangular window \n"); 
		break;
	case 2:	
		printf("Hamming window \n"); 
		break;
	case 3:	
		printf("Hanning window \n"); 
		break;
	case 4:	
		printf("Blackman window \n");
		break;
	case 5:	
		printf("Kaiser window \n");
		break;
	}
	printf("\n");

	printf("Impulse response coefficients\n");
	printf("\n");
	for (j=0;j<filt->n1;++j)
	{
		printf("h[%2d",j);
		printf("]  =\t%15.5e\t",h[filt->n1-j-1]);
		printf("    =  h[%2d",filt->n-j-1);
		printf("]\n");
		hb[j]=h[filt->n1-j-1]; 
		hb[filt->n-j-1]=hb[j];
	}
}

void filter_calc(filter_t *filt, double *hc, int hc_sz)
{
	int i;
	double *h=NULL,*w=NULL,*hb=NULL;
	filter_t loc_filt;

	memset(&loc_filt, 0, sizeof(filter_t));

	if (filt == NULL)
	{
		ENL();
		return;
	}

	if (hc == NULL)
	{
		ENL();
		return;
	}

	if (hc_sz<=0)
	{
		ENL();
		return;
	}

	//set internal structure value, precalculate coefficients
	filter_spec_(&loc_filt, filt->type, filt->fs, filt->fc1, filt->fc2, filt->wtype, filt->beta, filt->n);

	h  = malloc(sizeof(double) * loc_filt.n1);
	if (h==NULL)
	{
		ENL();
	}
	memset(h, 0, sizeof(double) * loc_filt.n1);

	hb = malloc(sizeof(double) * loc_filt.n);
	if (hb==NULL)
	{
		ENL();
	}
	memset(hb, 0, sizeof(double) * loc_filt.n);

	

	w  = malloc(sizeof(double) * loc_filt.n1);
	if (w==NULL)
	{
		ENL();
	}
	memset(w, 0, sizeof(double) * loc_filt.n1);

	if (
		(loc_filt.type == FILT_LP)
		|| (loc_filt.type == FILT_HP)
		)
	{
		lowpass_highpass_(&loc_filt, h);
	} else if (
		(loc_filt.type == FILT_BP)
		|| (loc_filt.type == FILT_BS)
		)
	{
		bandpass_bandstop_(&loc_filt, h, hb);
	} else
	{
		ENL();
	}

	window_func_(&loc_filt, w);   	/* compute the window function */
	for(i=0; i < loc_filt.n1; i++)
	{
		h[i]=h[i]*w[i];
	}
	//filter_output_(&loc_filt, h, hb); 	/* print the filter coefficients */
	
	for (i=0; i<loc_filt.n1; i++)
	{
		if (i<hc_sz)
		{
			hc[i]              = h[loc_filt.n1-i-1];

		}
		if ((loc_filt.n-i-1)<hc_sz)
		{
			hc[loc_filt.n-i-1] = h[loc_filt.n1-i-1];
		}
	}
	free(h);
	free(w);
	free(hb);

	return;
}


void *cmd_fir(void *data)
{
	char *ret = NULL;
	int i=0;

	int count;
	sds params;
	sds out_result;
	sds *tokens;

	double arg_fs=0.0;
	int arg_ftype=0;
	double arg_fc1=0.0;
	double arg_fc2=0.0;
	double arg_beta=0.0;
	int arg_wtype=0;
	int arg_n=0;
	double *lp_coef=NULL;
	filter_t filt;

	printf("FIR\n");

	if (data == NULL)
	{
		ret = alloc_new_str("FIR [FTYPE] [FS] [FC1] [FC2] [WTYPE] [BETA] [N]\n");
		return ret;
	}

	//prepare arguments
	params = sdsnew(data);
	out_result = sdsempty();
	tokens = sdssplitargs(params, &count);

	if (count == 1)
	{
		if (strncmp(tokens[0],"HELP",4)==0)
		{
			ret = alloc_new_str("FIR [Ftype=LP|HP|BP|BS] [Fs=1...1000000] [Fc1=1...1000000] [Fc2=1...1000000] [Wtype=RECT|HAMM|HANN|BLCK|KAIS] [beta=0.0...1.0] [n=1...63]\n");
		} else if (strncmp(tokens[0],"FIR",3)==0)
		{
			ret = alloc_new_str("Finite response filter\n");
		} else if (strncmp(tokens[0],"Ftype",5)==0)
		{
			ret = alloc_new_str("Filter type\n");
		} else if (strncmp(tokens[0],"Fs",2)==0)
		{
			ret = alloc_new_str("Sampling frequency\n");
		} else if (strncmp(tokens[0],"Fc1",3)==0)
		{
			ret = alloc_new_str("First cuttoff frequency\n");
		} else if (strncmp(tokens[0],"Fc2",3)==0)
		{
			ret = alloc_new_str("Second cuttoff frequency\n");
		} else if (strncmp(tokens[0],"Wtype",5)==0)
		{
			ret = alloc_new_str("Window type used to calculare\n");
		} else if (strncmp(tokens[0],"beta",4)==0)
		{
			ret = alloc_new_str("coeficient used with Kaiser window\n");
		} else if (strncmp(tokens[0],"n",1)==0)
		{
			ret = alloc_new_str("n-order filter\n");
		}
		sdsfree(params);
		sdsfree(out_result);
		sdsfreesplitres(tokens, count);
		return ret; 
	}

	if (count != 7)
	{
		PRINT("count = %d\n", count);
		ret = alloc_new_str("Need 7 arguments\nFIR [FTYPE] [FS] [FC1] [FC2] [WTYPE] [BETA] [N]\n");
		sdsfree(params);
		sdsfree(out_result);
		sdsfreesplitres(tokens, count);
		return ret; 
	}

	PNL();
	arg_fs = atof(tokens[1]);
	if (arg_fs>1000000)
	{
		arg_fs = 1000000; //1Mhz should be enought
	} else if (arg_fs<2)
	{
		arg_fs = 2;
	}

	PNL();
	arg_fc1 = atof(tokens[2]);
	if (arg_fc1>1000000.0)
	{
		arg_fc1 = 1000000.0;
	} else if (arg_fc1<1.0)
	{
		arg_fc1 = 1.0;
	}

	PNL();
	arg_fc1 = atof(tokens[2]);
	if (arg_fc1>1000000.0)
	{
		arg_fc1 = 1000000.0;
	} else if (arg_fs<1.0)
	{
		arg_fc1 = 1.0;
	}

	PNL();
	arg_n = atoi(tokens[6]);
	if (arg_n>63)
	{
		arg_n = 63;
	} else if (arg_n<1)
	{
		arg_n = 1;
	}

	PNL();
	arg_beta = atof(tokens[5]);
	if (arg_beta > 1.0)
	{
		arg_beta = 1.0;
	} else if (arg_beta < 0.0)
	{
		arg_beta = 0.0;
	}

	PNL();

	if (strncmp(tokens[4],"RECT",4)==0)
	{
		arg_wtype = FILT_WIN_RECT;
	} else if (strncmp(tokens[4],"HAMM",4)==0)
	{
		arg_wtype = FILT_WIN_HAMM;
	} else if (strncmp(tokens[4],"HANN",4)==0)
	{
		arg_wtype = FILT_WIN_HANN;
	} else if (strncmp(tokens[4],"BLCK",4)==0)
	{
		arg_wtype = FILT_WIN_BLCK;
	} else if (strncmp(tokens[4],"KAIS",4)==0)
	{
		arg_wtype = FILT_WIN_BLCK;
	} else
	{
		arg_wtype = FILT_WIN_RECT;
	}

	PNL();
	if (strncmp(tokens[0],"LP",2)==0)
	{
		arg_ftype = FILT_LP;
	} else if (strncmp(tokens[0],"HP",2)==0)
	{
		arg_ftype = FILT_HP;
	} else if (strncmp(tokens[0],"BP",2)==0)
	{
		arg_ftype = FILT_BP;
	} else if (strncmp(tokens[0],"BS",2)==0)
	{
		arg_ftype = FILT_BS;
	} else
	{
		arg_ftype = FILT_LP;
	}

	PNL();
	//calculate
	lp_coef = malloc(sizeof(double)*arg_n);
	memset(lp_coef, 0, sizeof(double)*arg_n);

	PNL();
	filt.type = arg_ftype;
	filt.fs = arg_fs;
	filt.fc1 = arg_fc1;
	filt.fc2 = arg_fc2;
	filt.wtype = arg_wtype;
	filt.n = arg_n;
	filt.beta = arg_beta;

	PNL();
	filter_calc(&filt, lp_coef, arg_n);


	//prepare output
	for (i=0;i<arg_n;i++)
	{
		char str_double[16];
		snprintf(str_double, 16, "%.3f ", lp_coef[i]);
		out_result = sdscat(out_result, str_double);
	}
	out_result = sdscat(out_result, "");

	ret = alloc_new_str(out_result);

	PNL();

	free(lp_coef);
	sdsfree(params);
	sdsfree(out_result);
	sdsfreesplitres(tokens, count);

	return ret;
}