#include "cmd_fir1p.h"

#define FIR_SIZE 12

double fir_coef[FIR_SIZE] = 
{
 -0.0044384,  -0.0041841,   0.0130183,   0.0746628,   
  0.1720210,   0.2489204,   0.2489204,   0.1720210,   
  0.0746628,   0.0130183,  -0.0041841,  -0.0044384
};

#define MAX_INPUT_LEN 80
#define BUFFER_LEN (FIR_SIZE-1+MAX_INPUT_LEN)
double insamp[BUFFER_LEN];

int fir1filter(double *coeffs, double *input, double *output,
	int length, int filter_length)
{
	double acc;
	double *coeffp;
	double *inputp;
	int n;
	int k;

	memcpy(&insamp[filter_length-1], input, length*sizeof(double));

	for (n=0; n<length; n++)
	{
		coeffp = coeffs;
		inputp = &insamp[filter_length - 1 + n];
		acc = 0;

		for (k=0;k<filter_length;k++)
		{
			acc += (*coeffp++)*(*inputp--);
		}

		output[n] = acc;
	}

	memmove(&insamp[0], &insamp[length], (filter_length-1)*sizeof(double));

	return 0;
}

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

	if (data == NULL)
	{
		ret = alloc_new_str("FIR filter N=12\n");
		return ret;
	}

	double input[MAX_INPUT_LEN];
	double output[MAX_INPUT_LEN];

	int count;
	sds params = sdsnew(data);
	sds out_result = sdsempty();
	sds *tokens;
	//printf("%s\n",params);
	//tokens = sdssplitlen(params, sdslen(params), " ", 1, &count);
	tokens = sdssplitargs(params, &count);

	for (i=0;i<count;i++)
	{
		//printf("[%s]\n",tokens[i]);
		input[i] = atof(tokens[i]);
	}

	fir1filter(fir_coef, input, output, count, FIR_SIZE);

	for (i=0;i<count;i++)
	{
		//cut to max size 512
		char str_double[16];
		snprintf(str_double, 16, "%.3f ", output[i]);
		out_result = sdscat(out_result, str_double);
	}
	out_result = sdscat(out_result,"");



	ret = alloc_new_str(out_result);

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

	return ret;
}