#include <psemek/audio/effect/butterworth.hpp>
#include <psemek/audio/effect/filter.hpp>
#include <psemek/audio/constants.hpp>
#include <psemek/math/constants.hpp>

namespace psemek::audio
{

	stream_ptr butterworth_low_pass(stream_ptr stream, float cutoff_frequency)
	{
		float omega = 2.f * static_cast<float>(math::pi) * cutoff_frequency * audio::inv_frequency;
		omega = 2.f * audio::frequency * std::tan(omega / 2.f);

		float gamma = 2.f * audio::frequency / omega;
		float sqrt2 = std::sqrt(2.f);

		float a0 = gamma * gamma + sqrt2 * gamma + 1.f;
		float a1 = - 2.f * gamma * gamma + 2.f;
		float a2 = gamma * gamma - sqrt2 * gamma + 1.f;

		float b0 = 1.f;
		float b1 = 2.f;
		float b2 = 1.f;

		a1 /= a0;
		a2 /= a0;
		b0 /= a0;
		b1 /= a0;
		b2 /= a0;

		return linear_filter(std::move(stream), {a1, a2}, {b0, b1, b2});
	}

	stream_ptr butterworth_high_pass(stream_ptr stream, float cutoff_frequency)
	{
		float omega = 2.f * static_cast<float>(math::pi) * (audio::frequency / 2.f - cutoff_frequency) * audio::inv_frequency;
		omega = 2.f * audio::frequency * std::tan(omega / 2.f);

		float gamma = 2.f * audio::frequency / omega;
		float sqrt2 = std::sqrt(2.f);

		float a0 = gamma * gamma + sqrt2 * gamma + 1.f;
		float a1 = 2.f * gamma * gamma - 2.f;
		float a2 = gamma * gamma - sqrt2 * gamma + 1.f;

		float b0 = 1.f;
		float b1 = - 2.f;
		float b2 = 1.f;

		a1 /= a0;
		a2 /= a0;
		b0 /= a0;
		b1 /= a0;
		b2 /= a0;

		return linear_filter(std::move(stream), {a1, a2}, {b0, b1, b2});
	}

}