#pragma once

#include "kdtree.h"
#include "point_cloud.h"
#include <glm/glm.hpp>
#include <vector>
#include <iostream>
#include <chrono>
#include "point_cloud.h"

using namespace std;
using namespace glm;
using namespace std::chrono;

namespace benchmark_tree {
	uint32_t elementBinarySearch(float s, const glm::vec3& sortAxis, vector<uint32_t> elements, Point* test_model);
	void test(uint32_t test_count, uint32_t reps)
	{
		cout << endl << endl << "...GENERATING TEST MODEL..." << endl << endl;
		Point* test_model = new Point[test_count];
		for (uint32_t i = 0; i < test_count; i++) {
			float x = (float)rand() / (float)RAND_MAX;
			float y = (float)rand() / (float)RAND_MAX;
			float z = (float)rand() / (float)RAND_MAX;
			test_model[i].position = vec3(x, y, z);
		}
		cout << endl;
		KdTree test_tree; 
		test_tree.SetData(test_model, test_count);
		cout << "DONE" << endl;

		cout << endl << endl << "::NEAREST NEIGHBOUR BENCHMARK::" << endl << endl;
		float averageTime1 = 0, averageTime2 = 0;
		float averageNodes1 = 0, averageNodes2 = 0;
		high_resolution_clock::time_point begin;
		high_resolution_clock::time_point end;
		duration<double, std::milli> time_span;

		for (uint32_t i = 0; i < reps; i++) {
			float x = (float)rand() / (float)RAND_MAX;
			float y = (float)rand() / (float)RAND_MAX;
			float z = (float)rand() / (float)RAND_MAX;
			vec3 randomPos(x, y, z);
			uint32_t randomInd = rand() % test_count;
			uint32_t r1, r2;

			vec3 pos = test_model[i].position;

			begin = high_resolution_clock::now();
			test_tree.NearestSearch(randomInd, r1);
			end = high_resolution_clock::now();
			time_span = end - begin;
			averageTime1 += time_span.count();
			averageNodes1 += test_tree._expandedNodes;

			begin = high_resolution_clock::now();
			test_tree.NearestSearch(pos, r2);
			end = high_resolution_clock::now();
			time_span = end - begin;
			averageTime2 += time_span.count();
			averageNodes2 += test_tree._expandedNodes;
		}

		averageTime1 /= reps;
		averageTime2 /= reps;
		averageNodes1 /= reps;
		averageNodes2 /= reps;

		cout << "FROM ANY POINT SEARCH:" << endl;
		cout << "  -Average time: " << averageTime2 << endl;
		cout << "  -Average expanded nodes: " << averageNodes2 << endl;
		cout << "  -Estimated nearest search for all model points: " << averageTime2 * test_count << endl;
		cout << endl;

		cout << "FROM MODEL POINT SEARCH:" << endl;
		cout << "  -Average time: " << averageTime1 << endl;
		cout << "  -Average expanded nodes: " << averageNodes1 << endl;
		cout << "  -Estimated nearest search for all model points: " << averageTime1 * test_count << endl;
		cout << endl;
	}

	void test2(uint32_t test_count, uint32_t reps) {
		high_resolution_clock::time_point tbegin;
		high_resolution_clock::time_point tend;
		duration<double, std::milli> time_span;
		cout << endl << endl << "...GENERATING TEST MODEL..." << endl << endl;
		Point* test_model = new Point[test_count];
		for (uint32_t i = 0; i < test_count; i++) {
			float x = (float)rand() / (float)RAND_MAX;
			float y = (float)rand() / (float)RAND_MAX;
			float z = (float)rand() / (float)RAND_MAX;
			test_model[i].position = vec3(x, y, z);
		}
		cout << endl;
		KdTree test_tree;
		test_tree.SetData(test_model, test_count);
		cout << "DONE" << endl;

		vector<uint32_t> elements;
		for (uint32_t i = 0; i < test_count; i++) {
			elements.push_back(i);
		}

		tbegin = high_resolution_clock::now();
		vec3 sortAxis(1,1,0);
		float slice_depth = 0.1;
		vector<PointCloudSlice> slices;
		std::sort(elements.begin(), elements.end(),
			[&sortAxis, test_model](uint32_t i, uint32_t j) {
				return dot(test_model[i].position, sortAxis) < dot(test_model[j].position, sortAxis);
			});
		tend = high_resolution_clock::now();
		time_span = tend - tbegin;
		cout << time_span.count() << endl;
		
		
		vector<uint32_t> results;
		float minValue;
		float maxValue;
		uint32_t tmp;
		tbegin = high_resolution_clock::now();
		test_tree.AxisMin(sortAxis, tmp, minValue);
		test_tree.AxisMax(sortAxis, tmp, maxValue);
		for (float f = minValue; f < maxValue; f += 0.001) {
			test_tree.AxisRange(sortAxis, f, f + 0.001, results);
		}
		tend = high_resolution_clock::now();
		time_span = tend - tbegin;
		cout << time_span.count() << endl;
	}

	uint32_t elementBinarySearch(float s, const glm::vec3& sortAxis, vector<uint32_t> elements, Point* test_model)
	{
		uint32_t from = 0;
		uint32_t to = (uint32_t)elements.size();
		while (from < to - 1) {
			uint32_t ind = (to + from) / 2;
			float v = glm::dot(test_model[elements[ind]].position, sortAxis);
			if (v < s) {
				from = ind;
			}
			else {
				to = ind;
			}
		}
		return from;
	}
}
