#include <string.h>
#include <math.h>
#include "filter.h"
#include "psyn.h"

void filter_init_rc(struct filter_t *filter, double R, double C)
{
	memset(filter, 0, sizeof *filter);
	filter->k = 1.0 / (R * C) / _sample_rate;
	filter->r = 1.0;
}

void filter_init_freq(struct filter_t *filter, double freq)
{
	memset(filter, 0, sizeof *filter);
	filter->k = (2.0 * M_PI * freq) / _sample_rate;
	filter->r = 1.0;
}

void filter_run_lp(struct filter_t *filter, double left, double right)
{
	filter->v_l += filter->k * (left - filter->last_out_l);
	filter->v_r += filter->k * (right - filter->last_out_r);
	filter->last_out_l = filter->v_l;
	filter->last_out_r = filter->v_r;
	filter->v_l *= filter->r;
	filter->v_r *= filter->r;
}

void filter_run_hp(struct filter_t *filter, double left, double right)
{
	filter->last_out_l = filter->k * (filter->last_out_l + left - filter->last_in_l);
	filter->last_out_r = filter->k * (filter->last_out_r + right - filter->last_in_r);
	filter->last_in_l = left;
	filter->last_in_r = right;
}

void bw_filter_init_lp(struct bw_filter_t *f, double freq, double resonance)
{
	double c = 1.0 / tan(M_PI * freq / _sample_rate);
	f->a1 = 1.0 / (1.0 + resonance * c + c * c);
	f->a2 = 2 * f->a1;
	f->a3 = f->a1;
	f->b1 = 2.0 * (1.0 - c * c) * f->a1;
	f->b2 = (1.0 - resonance * c + c * c) * f->a1;
}

void bw_filter_run(struct bw_filter_t *f, double in_l, double in_r, double *out_l, double *out_r)
{
	*out_l = f->a1 * in_l + f->a2 * f->in1_l + f->a3 * f->in2_l - f->b1 * f->out1_l - f->b2 * f->out2_l;
	*out_r = f->a1 * in_r + f->a2 * f->in1_r + f->a3 * f->in2_r - f->b1 * f->out1_r - f->b2 * f->out2_r;

	f->in2_l = f->in1_l;
	f->in1_l = in_l;
	f->out2_l = f->out1_l;
	f->out1_l = *out_l;

	f->in2_r = f->in1_r;
	f->in1_r = in_r;
	f->out2_r = f->out1_r;
	f->out1_r = *out_r;
}

/*
 The filter algo:
 out(n) = a1 * in + a2 * in(n-1) + a3 * in(n-2) - b1*out(n-1) - b2*out(n-2)

 Lowpass:
 c = 1.0 / tan(pi * f / sample_rate);

 a1 = 1.0 / ( 1.0 + r * c + c * c);
 a2 = 2* a1;
 a3 = a1;
 b1 = 2.0 * ( 1.0 - c*c) * a1;
 b2 = ( 1.0 - r * c + c * c) * a1;

 Hipass:
 c = tan(pi * f / sample_rate);

 a1 = 1.0 / ( 1.0 + r * c + c * c);
 a2 = -2*a1;
 a3 = a1;
 b1 = 2.0 * ( c*c - 1.0) * a1;
 b2 = ( 1.0 - r * c + c * c) * a1;
*/