require('pathfinding/vertex');
Astar = class.new();

function Astar:constructor(grid)
	self.grid	=	grid;
end


function Astar:findPath(startVertex, endVertex)
	local startNode;
	local endNode;
	local tmpStartNode	=	false;
	local tmpEndNode	=	false;

	-- If we already have start/end nodes, use those instead of creating temporary ones
	startNode 	=	self.grid:findNode(startVertex.x, startVertex.y);
	endNode		=	self.grid:findNode(endVertex.x, endVertex.y);
	
	if( not startNode ) then
		startNode		=	self.grid:addNode(Node(startVertex.x, startVertex.y));
		tmpStartNode	=	true;
	end

	if( not endNode ) then
		endNode			=	self.grid:addNode(Node(endVertex.x, endVertex.y));
		tmpEndNode		=	true;
	end

	local function removeTemporaryNodes()
		if( tmpStartNode ) then
			self.grid:removeNode(startNode);
		end
	if( tmpEndNode ) then
			self.grid:removeNode(endNode);
		end
	end


	local openSet	=	{};
	local closedSet	=	{};

	table.insert(openSet, startNode);
	while( #openSet > 0 ) do
		local node	=	openSet[1];

		--print("Compare movement costs...");
		-- Compare movement costs
		for i = 2, #openSet do
			if( openSet[i]:getFCost() <= node:getFCost() ) then
				if( openSet[i]:getHCost() < node:getHCost() ) then
					-- Total cost lower & closer to goal
					node	=	openSet[i];
				end
			end
		end

		-- Move node from open to closed
		table.remove(openSet, table.find(openSet, node));
		table.insert(closedSet, node);

		-- Check if we reached our goal
		if( node == endNode or node.x == endNode.x and node.y == endNode.y ) then
			-- retrace
			local path = self:retrace(startNode, endNode);
			removeTemporaryNodes();
			return path;
		end

		--print("Adding nodes from connections...");
		-- Grab connections, set costs
		for i,neighbor in pairs(node:getConnections()) do
			-- Make sure it's not already closed
			if( not table.find(closedSet, neighbor) ) then
				-- Calculate costs, add to open set
				local newCostToNeighbor = node:getGCost() + math.distance(node.x, node.y, neighbor.x, neighbor.y);
				if( newCostToNeighbor < neighbor:getGCost() or not table.find(openSet, neighbor) ) then
					neighbor:setGCost(newCostToNeighbor);
					neighbor:setHCost(math.distance(neighbor.x, neighbor.y, endNode.x, endNode.y));
					neighbor:setParent(node);

					if( not table.find(openSet, neighbor) ) then
						table.insert(openSet, neighbor);
					end
				end
			end
		end
	end

	removeTemporaryNodes();
	return false;
end

function Astar:retrace(startNode, endNode)
	local path = {};

	local function tableReverse(tab)
		-- Only have to swap first half of the table
		for i = 1, math.floor(#tab/2) do
			local tmp = tab[i];			-- Tmp copy this one.
			tab[i] = tab[#tab-i + 1];	-- Flip tab[i] and tab[n-i+1]; +1 because Lua tables start at index 1
			tab[#tab-i + 1] = tmp;
		end
		return tab;
	end

	--local startNode	=	self.grid:findNode(4, 0);--self.grid:addNode(Node(startVertex.x, startVertex.y));
	--local endNode	=	self.grid:findNode(4, 6);--self.grid:addNode(Node(endVertex.x, endVertex.y));

	local currentNode = endNode;
	while( currentNode ~= startNode ) do
		table.insert(path, currentNode);
		currentNode = currentNode.parent;
	end

	return tableReverse(path);
end