#include <psemek/audio/effect/filter.hpp>

namespace psemek::audio
{

	namespace
	{

		struct feedforward_filter_impl
			: stream
		{
			feedforward_filter_impl(stream_ptr stream, float a0, float a1)
				: stream_(std::move(stream))
				, a0_(a0)
				, a1_(a1)
			{}

			std::optional<std::size_t> length() const override
			{
				return stream_->length();
			}

			std::size_t read(util::span<float> samples) override
			{
				std::size_t count = stream_->read(samples);

				for (std::size_t i = 0; i < count; i += 2)
				{
					std::swap(prev_[0], samples[i + 0]);
					std::swap(prev_[1], samples[i + 1]);

					samples[i + 0] = prev_[0] * a0_ + samples[i + 0] * a1_;
					samples[i + 1] = prev_[1] * a0_ + samples[i + 1] * a1_;
				}

				return count;
			}

			std::size_t played() const override
			{
				return stream_->played();
			}

		private:
			stream_ptr stream_;
			float a0_;
			float a1_;
			float prev_[2]{0.f};
		};

		struct feedback_filter_impl
			: stream
		{
			feedback_filter_impl(stream_ptr stream, float a0, float b1)
				: stream_(std::move(stream))
				, a0_(a0)
				, b1_(b1)
			{}

			std::optional<std::size_t> length() const override
			{
				return stream_->length();
			}

			std::size_t read(util::span<float> samples) override
			{
				std::size_t count = stream_->read(samples);

				for (std::size_t i = 0; i < count; i += 2)
				{
					samples[i + 0] = prev_[0] * b1_ + samples[i + 0] * a0_;
					samples[i + 1] = prev_[1] * b1_ + samples[i + 1] * a0_;
					prev_[0] = samples[i + 0];
					prev_[1] = samples[i + 1];
				}

				return count;
			}

			std::size_t played() const override
			{
				return stream_->played();
			}

		private:
			stream_ptr stream_;
			float a0_;
			float b1_;
			float prev_[2]{0.f};
		};

	}

	stream_ptr feedforward_filter(stream_ptr stream, float a0, float a1)
	{
		return std::make_shared<feedforward_filter_impl>(std::move(stream), a0, a1);
	}

	stream_ptr feedback_filter(stream_ptr stream, float a0, float b1)
	{
		return std::make_shared<feedback_filter_impl>(std::move(stream), a0, b1);
	}

	stream_ptr low_pass_filter(stream_ptr stream)
	{
		return feedforward_filter(std::move(stream), 1.f, 1.f);
	}

	stream_ptr high_pass_filter(stream_ptr stream)
	{
		return feedforward_filter(std::move(stream), 1.f, -1.f);
	}

}
