package ei.game.algo;

import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;

import ei.game.algo.AStarNeighbour.Location;


/**
 * 
 * @author Árni Arent
 *
 */
public class AStarPathfinder{
	protected LinkedList<AStarNode> open;
	protected LinkedList<AStarNode> closed;
	private AStarNode start;
	private AStarNode goal;

	/**
	 * Constructs a Pathfinder.
	 */
	public AStarPathfinder() {
		this.open = new LinkedList<AStarNode>();
		this.closed = new LinkedList<AStarNode>();
	}
	
	/**
	 * 
	 * @param from the goal to go from, the start Node (origin).
	 * @param to the goal to go to, the destination Node.
	 * @return a List containing the path of Nodes to travel to reach goal.
	 */
	public List<AStarNode> findPath(AStarNode start, AStarNode goal) {
		this.start = start;
		this.goal = goal;
		
		return startSearch((AStarNode)start,(AStarNode)goal);
			
	}
	
	private List<AStarNode> startSearch(AStarNode start, AStarNode goal) {
		// Add the start node to the open list, initialize it.
		start.setParent(null); // make sure it does not have any parent defined.
		start.estimatedCostToGoal = start.getEstimatedCostTo(goal,start);
		start.costFromStart = 0;
		open.add(start);

		AStarNode node = null;
		// Go through all the items in the open storage.
		int order = 0; // defines the order of which the nodes were visited (used in gui visuals)
		while (!open.isEmpty()) {
			// Let's retrieve the first item from the storage.
			node = (AStarNode) open.removeFirst();
			node.setVisited(true);
			node.setVisitOrder(order++);

			// Check if we found the goal.
			if (node == goal) {
				return constructPath(node);
			} else {
				// Let's go through all the neighbours of this node.
				Iterator<AStarNeighbour> i = node.getNeighbours().iterator();
				while (i.hasNext()) {
					AStarNeighbour neighbour = (AStarNeighbour) i.next();
					AStarNode neighbourNode = (AStarNode)neighbour.getNode();
					
					/*
					 * We do not want to visit blocked neighbours, so we skip
					 * them. Also, if the neighbour node is neither in the
					 * closed and the open storage then add it to the open
					 * storage, and set it's parent.
					 */
					if(!neighbourNode.isBlocked()) {
						Location location = neighbour.getLocation();

						float costFromStart = node.costFromStart + node.getCost(neighbourNode, location);
						boolean inClosed = closed.contains(neighbourNode);
						boolean inOpen = open.contains(neighbourNode);
						
						if ((!inOpen && !inClosed) || costFromStart < neighbourNode.costFromStart) {
							neighbourNode.setParent(node);
							neighbourNode.costFromStart = costFromStart;
							neighbourNode.estimatedCostToGoal = neighbourNode.getEstimatedCostTo(goal,start);
	
							if (inClosed) {
								closed.remove(neighbourNode);
							}
							if (!inOpen) {
								open.add(neighbourNode);
							}
						}
					}
				}				
				closed.add(node);
			}
		}

		//return null;
		return constructPath(node);
	}


	/**
	 * Constructs a path from a Node through any number of 
	 * Nodes to the goal Node.
	 * 
	 * @param node the Node that contains the path back from the goal to the start.
	 * @return a List containing the path of Nodes to travel to reach goal.
	 */
	protected List<AStarNode> constructPath(AStarNode node) {
		LinkedList<AStarNode> path = new LinkedList<AStarNode>();
		
		while (node.getParent() != null) {
			node.setPartOfPath(true);
			path.addFirst(node);
			node = node.getParent();
		}
		
		return path;
	}
	
	/**
	 * Resets all the nodes from the previous search.
	 *
	 */
	protected void resetNodes() {
		if(goal != null) goal.reset();
		if(start != null) start.reset();
				
		if(open != null) {
			open.clear();
		}
		// Go through all the items in the open storage.
		if(closed != null) {
			closed.clear();
		}
	}
	
}