#include <iostream>

#include <vector>
#include <queue>
#include <chrono>
#include <limits>
#include <algorithm>
#include<cmath>

using namespace std::chrono;

typedef std::pair<double, int> idx_pair;
typedef std::priority_queue<idx_pair, std::vector<idx_pair>, std::greater<idx_pair>> idx_queue;

std::vector<idx_pair> getKSmallest(std::vector<double> const& data, int k)
{
	idx_queue q;
	{
		std::vector<idx_pair> idxPairs(data.size());
		for (auto i = 0; i < data.size(); i++)
			idxPairs[i] = idx_pair(data[i], i);

		q = idx_queue(std::begin(idxPairs), std::end(idxPairs));
	};

	std::vector<idx_pair> result;

	auto topPop = [&q, &result]()
	{
		result.push_back(q.top());
		q.pop();
	};

	for (auto i = 0; i < k; i++)
		topPop();

	auto const largest = result.back().first;
	while (q.empty() == false)
	{
		if (q.top().first == largest)
			topPop();
		else
			break;
	}

	return result;
}

void printVec(std::vector<idx_pair> const& data)
{
	for (auto const d : data)
		std::cout << "{" << d.second << "," << d.first << "} ";
		
	std::cout << std::endl;
}

bool equal(double value1, double value2)
{
	return value1 == value2 || std::abs(value2 - value1) <= std::numeric_limits<double>::epsilon();
}

std::vector<idx_pair> getNSmallest(std::vector<double> const& data, int n)
{
	std::vector<idx_pair> idxPairs(data.size());
	for (auto i = 0; i < data.size(); i++)
		idxPairs[i] = idx_pair(data[i], i);

	std::nth_element(std::begin(idxPairs), std::begin(idxPairs) + n, std::end(idxPairs));
	
	std::vector<idx_pair> result(std::begin(idxPairs), std::begin(idxPairs) + n);
	
	auto const largest = result.back().first;
	for (auto it = std::begin(idxPairs) + n; it != std::end(idxPairs); ++it)
	{
		if (equal(it->first, largest))
			result.push_back(*it);
	}

	return result;
}

int main() {
	std::vector<double> data = { 3.3, 1.1, 6.5, 4.2, 1.1, 3.5 };

	auto t1 = high_resolution_clock::now();
	auto const result = getNSmallest(data, 3);
	auto t2 = high_resolution_clock::now();

	std::cout << "time: " << duration_cast<milliseconds>(t2 - t1).count() << std::endl;
	
	printVec(result);

	return 0;
}