// Alexander Huddleston 223000555
// CSCE420
// Due: October 2, 2017 (changed to 4)
// hw1pr2.cpp

// Purpose: Read a file of points, compute interative best-first of a closed path between points.

#include<stdio.h>
#include<iostream>
#include<fstream>
#include<cmath>
#include<future>
#include<chrono>
#include<vector>
#include<climits>
#include<unordered_set>
#include<algorithm>

using namespace std;

// Double vector used to store list of squared distances between points.
vector<vector<long int>> dmat;

// Used to store the points from file.
vector<tuple<int, int>> points;

// Used to store the paths each iteration has traveled.
vector<unordered_set<long int>> relaxed_edges;

// Used to store the distance values of the respective paths.
vector<long int> shortest_paths;

void store_points(ifstream *input, string filename)
{
        input->open(filename);

        string line = "";

	string token = "";

	tuple<int, int> temp_point;
	get<0>(temp_point) = 0.0;
	get<1>(temp_point) = 0.0;

        while(getline(*input, line))
        {
                for(char c : line)
		{
			if(c == ' ')
			{
				get<0>(temp_point) = stof(token);
				token = "";
			}
			else
			{
				token = token + c;
			}
		}

		get<1>(temp_point) = stof(token);

		token = "";

		points.push_back(temp_point);
        }

	input->close();
}

void store_distances()
{
	long int d1 = 0.0;
	long int d2 = 0.0;

	tuple< int,  int> x;
	tuple< int,  int> y;
	get<0>(x) = 0.0;
	get<1>(x) = 0.0;
	get<0>(y) = 0.0;
	get<1>(y) = 0.0;

	vector<long int> tempvec;

	for(int r = points.size() - 1; r >= 0; --r)
	{
		for(int c = 0; c < points.size() - (points.size() - r); ++c)
		{
			x = points.at(r);
			y = points.at(c);

			d1 = (get<0>(x) - get<0>(y));
			d2 = (get<1>(x) - get<1>(y));

			tempvec.push_back(d1*d1 + d2*d2);
		}

		dmat.push_back(tempvec);
		tempvec.clear();
	}
}

bool tuple_sort(tuple<long int, long int> t1, tuple<long int, long int> t2)
{
	return (get<1>(t1) < get<1>(t2));
}

tuple<vector<int>, long double> iterate_path(int p)
{
	// Find best-first shortest path for this iteration.

	// Small optomization so we don't have to call points.size() so much.
	long int n = points.size();

	// In this variation, I want to keep track of the 4 closest points to root.
	vector<int> closest;
	vector<tuple<long int, long int>> first;
	long int temp_counter = n - 1;
	if(dmat.at(p).size() > 0)
	{
		for(long int b : dmat.at(p))
		{

			tuple<long int, long int> t (temp_counter, b);
			first.push_back(t);
			--temp_counter;
		}
	}
	for(int a = 0; a < (temp_counter); ++a)
	{
		tuple<long int, long int> t ((temp_counter - a), (dmat.at(a).at((n - 1) - p)));
		first.push_back(t);
	}
	sort(first.begin(), first.end(), tuple_sort);

	temp_counter = 0;
	for(tuple<long int, long int> te : first)
	{
		if(temp_counter > 3)
		{
			break;
		}
		closest.push_back(get<0>(te));
		++temp_counter;
	}

	// Store distance up to this point.
	long double current_distance = 0;

	// Using this to keep track of selected node.
	long int current_node = 0;

	long int selected_node = 0;

	// Using this to keep track of min distance.
	long int min_distance = 0;
	
	while(relaxed_edges.at(p).size() < (n - 1))
	{
		min_distance = INT_MAX;
		for(int i = 0; i < (n - 1); ++i)
		{
			while(relaxed_edges.at(p).find(i) != relaxed_edges.at(p).end())
			{
				++i;
				if(i == current_node)
				{
					++i;
				}
			}
			if(i > (n - 1))
			{
				break;
			}

			if(i < current_node)
			{
				if(min_distance > dmat.at(i).at((n - 1) - current_node))
				{
					selected_node = ((n - 1) - i);
					min_distance = dmat.at(i).at((n - 1) - current_node);
				}
			}
			else
			{
				if(i == current_node)
				{
					++i;
					if(i > (n - 1))
					{
						break;
					}
				}
				if(min_distance > dmat.at(current_node).at((n - 1) - i))
				{
					selected_node = ((n - 1) - i);
					min_distance = dmat.at(current_node).at((n - 1) - i);
				}
			}
		}

		relaxed_edges.at(p).insert((n - 1) - current_node);
		current_node = selected_node;
		current_distance += sqrt(min_distance);
	}

	current_distance += sqrt(dmat.at(current_node).at(0));
	
	tuple<vector<int>, long double> output;

	get<0>(output) = closest;
	get<1>(output) = current_distance;

	return output;
}

long double best_first(ifstream *input, string filename)
{
	// First, store the points from the file into global vector.
        store_points(input, filename);

	// Store squared distances into dmat.
	// Ignore duplicate distances, 0 vectors.
	store_distances();

	// This is how we know depth.
	long int depth = 1;

	// Which node iteration are we on?
	int n = 0;

	unordered_set<long int> empty;
	relaxed_edges.push_back(empty);

	while(depth < points.size() - 1)
	{
		// Find shortest path with given path.
		tuple<vector<int>, long double> ip = iterate_path(n);
		shortest_paths.push_back(get<1>(ip));
		vector<int> closest = get<0>(ip);

		unordered_set<long int> one;
		unordered_set<long int> two;
		unordered_set<long int> three;
		unordered_set<long int> four;

		long int temp = 0;

		for(long int i : relaxed_edges.at(n))
		{
			++temp;
			one.insert(i);
			two.insert(i);
			three.insert(i);
			four.insert(i);
			if(temp == depth)
			{
				if(closest.size() > 0)
				{
					one.insert(closest.at(0));
				}
				if(closest.size() > 1)
				{
					two.insert(closest.at(1));
				}
				if(closest.size() > 2)
				{
					three.insert(closest.at(2));
				}
				if(closest.size() > 3)
				{
					four.insert(closest.at(3));
				}
				break;
			}
		}

		relaxed_edges.push_back(one);
		relaxed_edges.push_back(two);
		relaxed_edges.push_back(three);
		relaxed_edges.push_back(four);

		if(n <= 1)
		{
			depth = 2;
		}
		else
		{
			depth = 2 + ((int)(log(n + 1)/log(4)));
		}

		++n;
	}

	return shortest_paths.at(0);
}

int main()
{
        ifstream input;
        
        string filename = "hw1pr2_data.txt";
        
        future<long double> fut = async(best_first, &input, filename);
        
        chrono::milliseconds span(60000);
        
        while((fut.wait_for(span)==future_status::timeout) || input.is_open())
        {
        }

        if(input.is_open())
        {
                input.close();
        }
	
	long double total = fut.get();

	tuple<int, int> tempx;
	tuple<int, int> tempy;

	long int temp = 0;

	// Change 0 to shortest path.
	for(long int i : relaxed_edges.at(0))
	{
		tempx = points.at(temp);
		tempy = points.at(i);
		cout << get<0>(tempx) << "\t" << get<1>(tempx) << "\t" << get<0>(tempy) << "\t" << get<1>(tempy) << endl;
		temp = i;
	}

	cout << total << endl;

        return 0;
}