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

#include "fftfilt.h"
#include "misc.h"

#undef	DEBUG

struct fftfilt *init_fftfilt(double f1, double f2, int len)
{
	struct fftfilt *s;
	double t, h, x;
	int i;

	if ((s = calloc(1, sizeof(struct fftfilt))) == NULL)
		return NULL;

	if ((s->fft = init_fft(len)) == NULL) {
		clear_fftfilt(s);
		return NULL;
	}

	if ((s->ifft = init_ifft(len)) == NULL) {
		clear_fftfilt(s);
		return NULL;
	}

	s->inbuf = calloc(len, sizeof(complex));
	s->outbuf = calloc(len, sizeof(complex));
	s->ovlbuf = calloc(len / 2, sizeof(complex));
	s->filter = calloc(len, sizeof(complex));

	if (!s->inbuf || !s->outbuf || !s->ovlbuf || !s->filter) {
		clear_fftfilt(s);
		return NULL;
	}

	s->filterlen = len;
	s->inptr = 0;

	len = s->filterlen / 2 + 1;

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

		x = (2 * f2 * sinc(2 * f2 * t) -
		     2 * f1 * sinc(2 * f1 * t)) * hamming(h);

		s->inbuf[i].re = x;
		s->inbuf[i].im = 0.0;
#ifdef DEBUG
                fprintf(stderr, "% e\t", x);
#endif
	}

	fft(s->fft, s->inbuf, s->filter);

#ifdef DEBUG
	for (i = 0; i < s->filterlen; i++)
		fprintf(stderr, "%e\n", 10 * log10(cpwr(s->filter[i])));
#endif

	/* clear inbuf as we used it above */
	memset(s->inbuf, 0, s->filterlen * sizeof(complex));

	return s;
}

void clear_fftfilt(struct fftfilt *s)
{
	if (s) {
		clear_fft(s->fft);
		clear_fft(s->ifft);
		free(s->inbuf);
		free(s->outbuf);
		free(s->ovlbuf);
		free(s->filter);
		free(s);
	}
}


/*
 * Filter with fast convolution (overlap-add algorithm).
 */
int fftfilt(struct fftfilt *s, complex in, complex **out)
{
	int i;

	/* collect filterlen/2 input samples */
	s->inbuf[s->inptr++] = in;

	if (s->inptr < s->filterlen / 2)
		return 0;

	/* FFT */
	fft(s->fft, s->inbuf, s->outbuf);

	/* multiply with the filter shape */
	for (i = 0; i < s->filterlen; i++)
		s->inbuf[i] = cmul(s->outbuf[i], s->filter[i]);

	/* IFFT */
	fft(s->ifft, s->inbuf, s->outbuf);

	/* overlap and add */
	for (i = 0; i < s->filterlen / 2; i++) {
		s->outbuf[i].re += s->ovlbuf[i].re;
		s->outbuf[i].im += s->ovlbuf[i].im;
	}
	*out = s->outbuf;

	/* save the second half for overlapping */
	memcpy(s->ovlbuf, s->outbuf + (s->filterlen / 2), (s->filterlen / 2) * sizeof(complex));

	/* clear inbuf */
	memset(s->inbuf, 0, s->filterlen * sizeof(complex));
	s->inptr = 0;

	/* signal the caller there is filterlen/2 samples ready */
	return s->filterlen / 2;
}
