#include <algorithm>

#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->explore(start);

	// no solution
	if (!this->is_visited(end)) return;

	XY pos = end;
	while (pos != start) {
		solution.push_front(pos);
		pos = this->map[pos].parent;
	}
}

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

void DijkstraPathfinder::explore(const XY & here, unsigned int cost) {
	if (this->is_visited(here)) return;

	PathfindingContext & ctx = this->museum.pathfinding;
	unsigned int end_cost = this->map_get(this->end).cost;

	vector<pair<unsigned int, XY>> neighbors;
	vector<XY> offsets = {
		{ 0, 1 },
		{ 1, 0 },
		{ 0, -1 },
		{ -1, 0 },
	};
	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 neighbor_cost = ctx.get_weight(neighbor_tile.data.type);
		unsigned int step_cost = cost + neighbor_cost;

		if (step_cost > end_cost) continue;

		neighbors.push_back({ step_cost, neighbor });

		Node & neighbor_node = this->map_get(neighbor);
		if (step_cost < neighbor_node.cost) {
			neighbor_node.parent = here;
			neighbor_node.cost = step_cost;
		}
	}

	this->set_visited(here);

	// sort by cost ascending
	sort(neighbors.begin(), neighbors.end(), [] (auto & left, auto & right) {
		return left.first < right.first;
	});

	for (pair<unsigned int, XY> & neighbor : neighbors) {
		this->explore(neighbor.second, neighbor.first);
	}
}