#include "PmergeMe.hpp"

template <typename Container>
Container PmergeMe::sort(Container &data, int level)
{
	// #1 Create pairs and sort them
	if (data.size() <= 1)
		return data;

	size_t group_size = 1 << level;

	int odd = data.size() % (2 * group_size);

	Container straggler = Container(data.end() - odd, data.end());

	data.erase(data.end() - odd, data.end());

	for (size_t i = 0; i < data.size(); i += (2 * group_size))
	{
		if (!less(data[i + group_size - 1], data[i + 2 * group_size - 1]))
			std::swap_ranges(
				data.begin() + i, data.begin() + i + group_size,
				data.begin() + i + group_size);
	}

	if (2 * group_size < data.size())
		data = sort(data, level + 1);

	Container main;
	Container pend;

	// #3 Separate main and pend
	main.insert(main.end(), data.begin(), data.begin() + group_size); // inserting b1
	auto it = data.begin() + group_size;							  // this is a1
	while (it != data.end())
	{
		main.insert(main.end(), it, it + group_size);
		it += group_size;
		if (it == data.end())
			break;
		pend.insert(pend.end(), it, it + group_size);
		it += group_size;
	}
	pend.insert(pend.end(), straggler.begin(), straggler.end());

	int i = 0;
	while (!pend.empty())
	{
		int search_window = std::min((1 << (i + 2)) - 1, static_cast<int>(main.size() / group_size));

		int jacob_index = Jacobstahl().get(i + 3) - Jacobstahl().get(i + 2) - 1;
		int first_to_insert = std::min(jacob_index, static_cast<int>(pend.size() / group_size) - 1);

		for (int to_insert = first_to_insert; to_insert >= 0; --to_insert)
			insert(main, pend, to_insert, search_window, group_size);

		i++;
	}
	return main;
}

template <typename Container>
void PmergeMe::insert(Container &main, Container &pend, int to_insert, int right, int group_size)
{
	if (pend.empty() || to_insert < 0 || to_insert >= static_cast<int>(pend.size()))
		return;

	int value_to_insert = pend[(to_insert + 1) * group_size - 1];
	auto insert_pos = main.begin() + right * group_size;

	int left = 0;
	while (left < right)
	{
		int mid = left + (right - left) / 2;
		int last_idx = std::min((mid + 1) * group_size, static_cast<int>(main.size())) - 1;
		if (less(main[last_idx], value_to_insert))
			left = mid + 1;
		else
			right = mid;
	}
	insert_pos = main.begin() + (left * group_size);

	main.insert(insert_pos, pend.begin() + to_insert * group_size, pend.begin() + (to_insert + 1) * group_size);
	pend.erase(pend.begin() + to_insert * group_size, pend.begin() + (to_insert + 1) * group_size);
}