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

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

void filter_init_freq(struct filter_t *filter, double freq)
{
	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;
}