#include "../../include/dv-processing/kinematics/motion_compensator.hpp"

#include <benchmark/benchmark.h>

#include <random>

static const size_t eventCount = 100000;
static const cv::Size resolution(640, 480);

static void bmMotionCompensatorEventRate(benchmark::State &state) {
	dv::kinematics::MotionCompensator accumulator(resolution);
	dv::EventStore store;

	std::default_random_engine generator;
	std::uniform_real_distribution<double> distribution(0.0, 1.0);

	for (size_t i = 0; i < eventCount; i++) {
		store.emplace_back(1000, distribution(generator) * resolution.width,
			distribution(generator) * resolution.height, distribution(generator) > 0.5);
	}

	accumulator.accept(dv::kinematics::Transformationf(500, Eigen::Matrix4f::Identity()));
	accumulator.accept(dv::kinematics::Transformationf(1000, Eigen::Matrix4f::Identity()));
	accumulator.accept(dv::kinematics::Transformationf(1500, Eigen::Matrix4f::Identity()));

	accumulator.accept(dv::measurements::Depth(1000, 3.f));

	size_t counter = 0;
	for (auto _ : state) {
		accumulator.accept(store);
		accumulator.generateFrame();
		counter += store.size();
	}
	state.SetItemsProcessed(static_cast<int64_t>(counter));
	state.SetLabel("events");
}

static void bmMotionCompensatorFrameRate(benchmark::State &state) {
	dv::kinematics::MotionCompensator accumulator(resolution);
	dv::EventStore store;

	std::default_random_engine generator;
	std::uniform_real_distribution<double> distribution(0.0, 1.0);

	for (size_t i = 0; i < eventCount; i++) {
		store.emplace_back(1000, distribution(generator) * resolution.width,
			distribution(generator) * resolution.height, distribution(generator) > 0.5);
	}

	accumulator.accept(dv::kinematics::Transformationf(500, Eigen::Matrix4f::Identity()));
	accumulator.accept(dv::kinematics::Transformationf(1000, Eigen::Matrix4f::Identity()));
	accumulator.accept(dv::kinematics::Transformationf(1500, Eigen::Matrix4f::Identity()));

	accumulator.accept(dv::measurements::Depth(1000, 3.f));

	int64_t counter = 0;
	for (auto _ : state) {
		accumulator.accept(store);
		accumulator.generateFrame();
		counter++;
	}
	state.SetItemsProcessed(counter);
	state.SetLabel("frames");
}

int main(int argc, char **argv) {
	BENCHMARK(bmMotionCompensatorFrameRate);
	BENCHMARK(bmMotionCompensatorEventRate);

	::benchmark::Initialize(&argc, argv);
	if (::benchmark::ReportUnrecognizedArguments(argc, argv)) {
		return 1;
	}
	::benchmark::RunSpecifiedBenchmarks();

	return EXIT_SUCCESS;
}
