#include <cstdlib>
#include <chrono>
#include <kfr/dsp/biquad.hpp>

/* Example g++ compile: g++ -Wall -O3 -fomit-frame-pointer -mavx2 -march=native -I kfr/include -std=gnu++17 kfrtest.cpp */
/* Example to run:  ./a.out 0 100000000 */

/* Simple biquadratic filter with normalized a0 term */
struct simple_filter {
	float a1,a2, b0,b1,b2;
};

/* Straightforard C code to apply filter */
void simple_apply(const simple_filter& f, float* __restrict data, size_t len) {
	float z1 = 0, z2 = 0;
	for (size_t i = 0; i < len; i++) {
		float out = f.b0 * data[i]              + z1;
		       z1 = f.b1 * data[i] - f.a1 * out + z2;
		       z2 = f.b2 * data[i] - f.a2 * out;
		data[i] = out;
	}
}

/* Create biquad filter chain with 1 filter */
auto kfr_create(float a1, float a2, float b0, float b1, float b2) {
	kfr::biquad_params<float> params[1] {
		{1.0, a1, a2, b0, b1, b2},
	};
	return kfr::biquad_filter<float, 1>{params};
}


void kfr_apply(kfr::biquad_filter<float, 1>& f, float* __restrict data, size_t len)
{
	f.reset();
	f.apply(data, len);
}

/* Some sample parameters */
#define HFPCOEFF -1.407505, 0.546649, 0.738539, -1.477077, 0.738539

int main(int argc, const char* argv[]) {

	if (argc < 3) { printf("Usage: %s [0|1] [size]\n", argv[0]); return -1; }

	const int which = atoi(argv[1]);
	const size_t len = atoi(argv[2]);
	const unsigned iters = (320000000 / len) + 1;
	alignas(32) float * __restrict data = new(std::align_val_t(32)) float[len];
	for (size_t i = 0; i < len; i++) data[i] = (float)rand() / RAND_MAX;
	printf("Algorithm %d, %zu samples, %u iterations\n", which, len, iters);

	simple_filter hpf { HFPCOEFF };
	auto kfr_hpf = kfr_create(HFPCOEFF);

	auto start = std::chrono::high_resolution_clock::now();
	/* Simple C version */
	if (which == 0)
		for (unsigned i = 0; i < iters; i++)
			simple_apply(hpf, data, len);
	/* using KFR filter */
	if (which == 1)
		for (unsigned i = 0; i < iters; i++)
			kfr_apply(kfr_hpf, data, len);
	auto stop = std::chrono::high_resolution_clock::now();
	printf("Total time %.3f ms\n", std::chrono::duration<double, std::milli>(stop - start).count());

	return 0;
}