#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "cmplx.h"
#include "filter.h"

#undef	DEBUG

#ifdef DEBUG
#include <stdio.h>
#endif

/*
 * This gets used when not optimising
 */
float mac(const float *a, const float *b)
{ 
        float sum = 0;
        unsigned int i;

        for (i = 0; i < FilterLen; i++)
                sum += (*a++) * (*b++);
        return sum;
}

/*
 * Sinc done properly.
 */
static inline double sinc(double x)
{
	if (fabs(x) < 1e-10)
		return 1.0;
	else
		return sin(M_PI * x) / (M_PI * x);
}

/*
 * Don't ask...
 */
static inline double cosc(double x)
{
	if (fabs(x) < 1e-10)
		return 0.0;
	else
		return (1.0 - cos(M_PI * x)) / (M_PI * x);
}

/*
 * Hamming window function.
 */
static inline double hamming(double x)
{
	return 0.54 - 0.46 * cos(2 * M_PI * x);
}

/*
 * Create a band pass Hilbert transformer / filter with 6 dB corner
 * frequencies of 'f1' and 'f2'. (0 <= f1 < f2 <= 0.5)
 */
struct filter *init_filter(float f1, float f2)
{
	struct filter *f;
	float t, h, x;
	int i;

	if ((f = calloc(1, sizeof(struct filter))) == NULL)
		return NULL;

#ifdef DEBUG
	fprintf(stderr, "#\n# f1=%f\tf2=%f\tn=%d\n#\n", f1, f2, FilterLen);
#endif

	for (i = 0; i < FilterLen; i++) {
		t = i - (FilterLen - 1.0) / 2.0;
		h = i * (1.0 / (FilterLen - 1.0));

		x = (2 * f2 * sinc(2 * f2 * t) -
		     2 * f1 * sinc(2 * f1 * t)) * hamming(h);
		f->ifilter[i] = x;
#ifdef DEBUG
		fprintf(stderr, "% e\t", x);
#endif

		/*
		 * The actual filter code assumes the impulse response
		 * is in time reversed order. This will be anti-
		 * symmetric so the minus sign handles that for us.
		 */
		x = (2 * f2 * cosc(2 * f2 * t) -
		     2 * f1 * cosc(2 * f1 * t)) * hamming(h);
		f->qfilter[i] = -x;
#ifdef DEBUG
		fprintf(stderr, "% e\n", x);
#endif
	}

	f->ptr = FilterLen;

#ifdef DEBUG
	fprintf(stderr, "#\n");
#endif

	return f;
}

void clear_filter(struct filter *f)
{
	free(f);
}

complex filter(struct filter *f, complex in)
{
	float *iptr = f->ibuffer + f->ptr;
	float *qptr = f->qbuffer + f->ptr;
	complex out;

	*iptr = in.re;
	*qptr = in.im;

	out.re = mac(iptr - FilterLen, f->ifilter);
	out.im = mac(qptr - FilterLen, f->qfilter);

	f->ptr++;
	if (f->ptr == BufferLen) {
		iptr = f->ibuffer + BufferLen - FilterLen;
		qptr = f->qbuffer + BufferLen - FilterLen;
		memcpy(f->ibuffer, iptr, FilterLen * sizeof(float));
		memcpy(f->qbuffer, qptr, FilterLen * sizeof(float));
		f->ptr = FilterLen;
	}

	return out;
}

float filterI(struct filter *f, float in)
{
	float *iptr = f->ibuffer + f->ptr;
	float out;

	*iptr = in;

	out = mac(iptr - FilterLen, f->ifilter);

	f->ptr++;
	if (f->ptr == BufferLen) {
		iptr = f->ibuffer + BufferLen - FilterLen;
		memcpy(f->ibuffer, iptr, FilterLen * sizeof(float));
		f->ptr = FilterLen;
	}

	return out;
}

float filterQ(struct filter *f, float in)
{
	float *qptr = f->qbuffer + f->ptr;
	float out;

	*qptr = in;

	out = mac(qptr - FilterLen, f->qfilter);

	f->ptr++;
	if (f->ptr == BufferLen) {
		qptr = f->qbuffer + BufferLen - FilterLen;
		memcpy(f->qbuffer, qptr, FilterLen * sizeof(float));
		f->ptr = FilterLen;
	}

	return out;
}
