#include <algorithm>
#include <queue>

#include "DijkstraPathfinder.h"
#include "Museum.h"

using namespace std;

const DijkstraPathfinder::Path & DijkstraPathfinder::get_path() {
	return this->solution;
}

void DijkstraPathfinder::clear() {
	Pathfinder::clear();
	this->solution.clear();
	this->map.clear();
}

void DijkstraPathfinder::find_between(const XY & start, const XY & end) {
	this->clear();
	this->end = end;

	this->map_get(start).distance = 0;
	this->dijkstra(start);

	if (!this->is_visited(end)) {
		printf("Dijkstra: no solution found\n");
		return;
	}

	XY pos = end;
	int steps = 0;
	while (pos != start) {
		solution.push_front(pos);
		pos = this->map_get(pos).parent;
		steps++;
	}
	printf("Dijkstra: solution found (%d steps, %u time)\n", steps, this->map_get(end).distance);
}

DijkstraPathfinder::Node & DijkstraPathfinder::map_get(const XY & pos) {
	if (!this->map.contains(pos))
		this->map[pos] = Node();
	return this->map[pos];
}

void DijkstraPathfinder::dijkstra(const XY & start) {
	PathfindingContext & ctx = this->museum.pathfinding;

	const vector<XY> offsets = {
		{ 0, 1 },
		{ 1, 0 },
		{ 0, -1 },
		{ -1, 0 },
	};

	auto compare = [] (Neighbor a, Neighbor b) -> bool { return a.distance > b.distance; };
	priority_queue<Neighbor, vector<Neighbor>, decltype(compare)> pq;

	pq.push({ 0, start });
	this->map_get(start).distance = 0;

	while (!pq.empty()) {
		XY here = pq.top().position;
		unsigned int distance = pq.top().distance;
		pq.pop();

		if (this->is_visited(here)) continue;
		this->set_visited(here);

		if (distance > this->map_get(here).distance) continue;

		unsigned int dist_end = this->map_get(this->end).distance;

		for (const XY & offset : offsets) {
			const XY neighbor = here + offset;
			if (ctx.empty_point(neighbor)) continue;

			Tile & neighbor_tile = this->museum.canvas.get_tile(neighbor);
			unsigned int dist_neighbor = distance + ctx.get_weight(neighbor_tile.data.type);

			if (dist_neighbor >= dist_end) continue;

			Node & neighbor_node = this->map_get(neighbor);
			if (dist_neighbor >= neighbor_node.distance) continue;

			neighbor_node.parent = here;
			neighbor_node.distance = dist_neighbor;

			pq.push({
				.distance = dist_neighbor,
				.position = neighbor,
			});
		}
	}
}