#include <map>
#include <queue>
#include <set>
struct vector2D {
	int x, y;
	bool operator < (vector2D b) const
	{
		return std::make_pair(x, y) > std::make_pair(b.x, b.y);
	}
	bool operator != (vector2D b) const
	{
		return (x != b.x || y != b.y);
	}
	bool operator == (vector2D b) const
	{
		return (x == b.x && y == b.y);
	}
};
template <class t>
double pythagorean(t a, t b) {
	return sqrt(pow(b.x - a.x, 2) + pow(b.y - a.y, 2));
};

class node {
public:
	double f;
	double g;
	vector2D pos;
	bool isClosed;
	bool operator < (node b) const {
		return b.f < f;
	};
	bool operator == (node b) const {
		pos == b.pos;
	}
	node() { g = INT_MAX;  isClosed = false; };
};
std::set<vector2D> neighbors(vector2D currentPos)
{

	std::set<vector2D> toReturn;
	toReturn.insert({ currentPos.x - 1, currentPos.y - 1 });
	toReturn.insert({ currentPos.x - 0, currentPos.y - 1 });
	toReturn.insert({ currentPos.x + 1, currentPos.y - 1 });
	toReturn.insert({ currentPos.x - 1, currentPos.y - 0 });
	toReturn.insert({ currentPos.x + 1, currentPos.y - 0 });
	toReturn.insert({ currentPos.x - 1, currentPos.y + 1 });
	toReturn.insert({ currentPos.x - 0, currentPos.y + 1 });
	toReturn.insert({ currentPos.x + 1, currentPos.y + 1 });

	return toReturn;
};
int main() {
	vector2D start = { 0,0 };
	vector2D finish = { 0,40};

	std::priority_queue<node> openSet;
	std::map<vector2D, node*> createdNodes;
	node startNode;
	startNode.f = pythagorean(start, finish);
	startNode.g = 0;
	startNode.pos = start;
	createdNodes[start] = &startNode;
	openSet.push(startNode);
	while (!openSet.empty())
	{
		node currentNode = openSet.top();
		vector2D currentPos = currentNode.pos;
		if (currentPos == finish) {
			printf("Found Path!\n");
			break;
		}
		openSet.pop();
		createdNodes[currentPos]->isClosed = true;


		for (auto n : neighbors(currentPos)) {

			node* neighbor = nullptr;

			switch (createdNodes.count(n)) {
			case 0:
				neighbor = &node();
				createdNodes[n] = neighbor;

				neighbor->pos = n;
				break;
			case 1:
				neighbor = createdNodes[n];
				break;
			}
			printf("current node is at %i, %i\n", neighbor->pos.x, neighbor->pos.y);
			if (neighbor->isClosed) {
				continue;
			}

			double tenativeGScore = currentNode.g + pythagorean(currentPos, neighbor->pos);

			neighbor->g = tenativeGScore;
			neighbor->f = pythagorean(neighbor->pos, finish);
			openSet.push(*neighbor);
		}
	}
	while(true){}
}