/* ScummVM - Graphic Adventure Engine
 *
 * ScummVM is the legal property of its developers, whose names
 * are too numerous to list here. Please refer to the COPYRIGHT
 * file distributed with this source distribution.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "engines/stark/movement/shortestpath.h"

#include "common/hash-ptr.h"

#include "engines/stark/resources/floor.h"

namespace Stark {

ShortestPath::NodeList ShortestPath::search(const Resources::FloorEdge *start, const Resources::FloorEdge *goal) {
	NodeList frontier;
	NodePrecedenceMap cameFrom;
	NodeCostMap costSoFar;

	frontier.push_back(start);
	cameFrom[start] = nullptr;
	costSoFar[start] = 0;

	while (!frontier.empty()) {
		const Resources::FloorEdge *current = popEdgeWithLowestCost(frontier, costSoFar);

		if (current == goal)
			break;

		Common::Array<Resources::FloorEdge *> neighbours = current->getNeighbours();
		for (uint i = 0; i < neighbours.size(); i++) {
			const Resources::FloorEdge *next = neighbours[i];
			if (!next->isEnabled())
				continue;

			float newCost = costSoFar[current] + current->costTo(next);
			if (!costSoFar.contains(next) || newCost < costSoFar[next]) {
				frontier.push_back(next);
				cameFrom[next] = current;
				costSoFar[next] = newCost;
			}
		}
	}

	return rebuildPath(start, goal, cameFrom);
}

ShortestPath::NodeList ShortestPath::rebuildPath(const Resources::FloorEdge *start, const Resources::FloorEdge *goal,
												 const NodePrecedenceMap &cameFrom) const {
	NodeList path;

	const Resources::FloorEdge *current = goal;
	path.push_front(goal);

	while (current && current != start) {
		current = cameFrom.getValOrDefault(current, nullptr);
		path.push_front(current);
	}

	if (current != start) {
		// No path has been found from start to goal
		return NodeList();
	}

	path.push_front(start);
	return path;
}

const Resources::FloorEdge *ShortestPath::popEdgeWithLowestCost(NodeList &frontier, const NodeCostMap &costSoFar) const {
	// Poor man's priority queue using a list ...
	NodeList::iterator lowestCostItem = frontier.begin();
	for (NodeList::iterator it = frontier.begin(); it != frontier.end(); it++) {
		if (costSoFar[*it] < costSoFar[*lowestCostItem]) {
			lowestCostItem = it;
		}
	}

	const Resources::FloorEdge *result = *lowestCostItem;

	frontier.erase(lowestCostItem);

	return result;
}

} // End of namespace Stark