#include "FilterWorker.h"
#include "SampleBlock.h"

FilterWorker::FilterWorker(SampleBlock& input, SampleBlock& output) {
	_input = &input;
	_output = &output;
}

void FilterWorker::filter(Coefficients bass, Coefficients treble) {
  for (size_t i = 0; i < _output->size; i++) {
    double x_pp = i < 2 ? 0 : *(_input->begin + i - 2);
    double x_p = i < 1 ? 0 : *(_input->begin + i - 1);
    double x_ = i < 0 ? 0 : *(_input->begin + i - 0);
    double y_pp = i < 2 ? 0 : *(_output->begin + i - 2);
    double y_p = i < 1 ? 0 : *(_output->begin + i - 1);
    *(_output->begin + i) = bass.b0 * x_ +
		                        bass.b1 * x_p +
		                        bass.b2 * x_pp +
		                        bass.a1 * y_p +
		                        bass.a2 * y_pp;
  }
  for (size_t i = 0; i < _output->size; i++) {
    double x_pp = i < 2 ? 0 : *(_input->begin + i - 2);
    double x_p = i < 1 ? 0 : *(_input->begin + i - 1);
    double x_ = i < 0 ? 0 : *(_input->begin + i - 0);
    double y_pp = i < 2 ? 0 : *(_output->begin + i - 2);
    double y_p = i < 1 ? 0 : *(_output->begin + i - 1);
    *(_output->begin + i) = treble.b0 * x_ +
		                        treble.b1 * x_p +
		                        treble.b2 * x_pp +
		                        treble.a1 * y_p +
		                        treble.a2 * y_pp;
  }
}