#pragma once

#include <array>
#include <algorithm>
#include <cmath>
#include <type_traits>
#include <limits>
#include "resample.h"
#include "filter.h"

namespace {
	template<typename result_type, typename type>
	constexpr result_type dither_truncate(type value, unsigned dither)
	{
		using limits = std::numeric_limits<result_type>;
		using dither_limits = std::numeric_limits<decltype(dither)>;
		if constexpr(std::is_floating_point_v<type>)
		{
			value += dither * type{1} / type{1ul << dither_limits::digits};
			value = std::floor(value);
		}
		else
		{
			static_assert(std::is_integral_v<type>, "Unsupported acc_type");

			value += dither >> (dither_limits::digits - limits::digits); // 17 for short
			value >>= limits::digits; // 15 for short
		}

		return std::clamp<type>(value, limits::min(), limits::max());
	}
}

template<typename sample_type_in, typename sample_type_out, typename filter_type, typename acc_type, size_t channels = 1, size_t INTERP_BIT = 14>
void lfr_resample(lfr_fixed_t& pos, const lfr_fixed_t inv_ratio, unsigned& dither, void* out, std::size_t outlen, const void* in, std::size_t inlen, const lfr_filter& filter)
{
	static_assert(channels > 0, "0 channels do not make any sense");

	auto inp = reinterpret_cast<const sample_type_in*>(in);
	auto outp = reinterpret_cast<sample_type_out*>(out);
	auto fd = reinterpret_cast<const filter_type*>(filter.data.data());

	const auto flen = filter.nsamp;
	const auto log2nfilt = filter.log2nfilt;
	auto x = pos;
	std::array<unsigned, channels> ds{dither};
	for(std::size_t i = 1; i < channels; ++i)
	{
		ds[i] = LCG_A * ds[i - 1] + LCG_C;
	}

	for(std::size_t i = 0; i < outlen; ++i)
	{
		/* fn: filter number
			ff0: filter factor for filter fn
			ff1: filter factor for filter fn+1 */

		const auto fn = (static_cast<unsigned>(x >> 1) >> (31 - log2nfilt)) & ((1u << log2nfilt) - 1);
		int ff1 = (static_cast<unsigned>(x >> (32 - log2nfilt - INTERP_BIT))) & ((1u << INTERP_BIT) - 1);
		int ff0 = (1u << INTERP_BIT) - ff1;

		/* off: offset in input corresponding to first sample in filter */
		auto off = static_cast<int>(x >> 32);
		/* fidx0, fidx1: start, end indexes in FIR data */
		int fidx0 = std::max(-off, 0);
		int fidx1 = std::min<int>(inlen - off, flen);

		/* acc: FIR accumulator */
		std::array<acc_type, channels> acc{0};

		for(int j = fidx0; j < fidx1; ++j)
		{
			acc_type f = (fd[(fn + 0) * flen + j] * ff0 + fd[(fn + 1) * flen + j] * ff1) / (1 << INTERP_BIT);
			for(std::size_t c = 0; c < channels; ++c)
			{
				acc[c] += inp[(j + off) * channels + c] * f;
			}
		}

		for(std::size_t c = 0; c < channels; ++c)
		{
			outp[i * channels + c] = dither_truncate<sample_type_out>(acc[c], ds[c]);

			ds[c] = LCG_A2 * ds[c] + LCG_C2;
		}

		x += inv_ratio;
	}

	pos = x;
	dither = ds[0];
}