package org.anddev.andengine.util.path.astar;

import java.util.ArrayList;
import java.util.Collections;

import org.anddev.andengine.util.path.IPathFinder;
import org.anddev.andengine.util.path.ITiledMap;
import org.anddev.andengine.util.path.Path;

/**
 * (c) 2010 Nicolas Gramlich 
 * (c) 2011 Zynga Inc.
 * 
 * @author Nicolas Gramlich
 * @since 23:16:17 - 16.08.2010
 */
public class AStarPathFinder<T> implements IPathFinder<T> {
	// ===========================================================
	// Constants
	// ===========================================================

	// ===========================================================
	// Fields
	// ===========================================================

	private final ArrayList<Node> mVisitedNodes = new ArrayList<Node>();
	private final ArrayList<Node> mOpenNodes = new ArrayList<Node>();

	private final ITiledMap<T> mTiledMap;
	private final int mMaxSearchDepth;

	private final Node[][] mNodes;
	private final boolean mAllowDiagonalMovement;

	private final IAStarHeuristic<T> mAStarHeuristic;

	// ===========================================================
	// Constructors
	// ===========================================================

	public AStarPathFinder(final ITiledMap<T> pTiledMap, final int pMaxSearchDepth, final boolean pAllowDiagonalMovement) {
		this(pTiledMap, pMaxSearchDepth, pAllowDiagonalMovement, new EuclideanHeuristic<T>());
	}

	public AStarPathFinder(final ITiledMap<T> pTiledMap, final int pMaxSearchDepth, final boolean pAllowDiagonalMovement, final IAStarHeuristic<T> pAStarHeuristic) {
		this.mAStarHeuristic = pAStarHeuristic;
		this.mTiledMap = pTiledMap;
		this.mMaxSearchDepth = pMaxSearchDepth;
		this.mAllowDiagonalMovement = pAllowDiagonalMovement;

		this.mNodes = new Node[pTiledMap.getTileRows()][pTiledMap.getTileColumns()];
		final Node[][] nodes = this.mNodes;
		for(int x = pTiledMap.getTileColumns() - 1; x >= 0; x--) {
			for(int y = pTiledMap.getTileRows() - 1; y >= 0; y--) {
				nodes[y][x] = new Node(x, y);
			}
		}
	}

	// ===========================================================
	// Getter & Setter
	// ===========================================================

	// ===========================================================
	// Methods for/from SuperClass/Interfaces
	// ===========================================================

	@Override
	public Path findPath(final T pEntity, final int pMaxCost, final int pFromTileColumn, final int pFromTileRow, final int pToTileColumn, final int pToTileRow) {
		final ITiledMap<T> tiledMap = this.mTiledMap;
		if(tiledMap.isTileBlocked(pEntity, pToTileColumn, pToTileRow)) {
			return null;
		}

		/* Drag some fields to local variables. */
		final ArrayList<Node> openNodes = this.mOpenNodes;
		final ArrayList<Node> visitedNodes = this.mVisitedNodes;

		final Node[][] nodes = this.mNodes;
		final Node fromNode = nodes[pFromTileRow][pFromTileColumn];
		final Node toNode = nodes[pToTileRow][pToTileColumn];

		final IAStarHeuristic<T> aStarHeuristic = this.mAStarHeuristic;
		final boolean allowDiagonalMovement = this.mAllowDiagonalMovement;
		final int maxSearchDepth = this.mMaxSearchDepth;

		/* Initialize algorithm. */
		fromNode.mCost = 0;
		fromNode.mDepth = 0;
		toNode.mParent = null;

		visitedNodes.clear();

		openNodes.clear();
		openNodes.add(fromNode);

		int currentDepth = 0;
		while(currentDepth < maxSearchDepth && !openNodes.isEmpty()) {
			/* The first Node in the open list is the one with the lowest cost. */
			final Node current = openNodes.remove(0);
			if(current == toNode) {
				break;
			}

			visitedNodes.add(current);

			/* Loop over all neighbors of this tile. */
			for(int dX = -1; dX <= 1; dX++) {
				for(int dY = -1; dY <= 1; dY++) {
					if((dX == 0) && (dY == 0)) {
						continue;
					}

					if(!allowDiagonalMovement) {
						if((dX != 0) && (dY != 0)) {
							continue;
						}
					}

					final int neighborTileColumn = dX + current.mTileColumn;
					final int neighborTileRow = dY + current.mTileRow;

					if(!this.isTileBlocked(pEntity, pFromTileColumn, pFromTileRow, neighborTileColumn, neighborTileRow)) {
						final float neighborCost = current.mCost + tiledMap.getStepCost(pEntity, current.mTileColumn, current.mTileRow, neighborTileColumn, neighborTileRow);
						final Node neighbor = nodes[neighborTileRow][neighborTileColumn];
						tiledMap.onTileVisitedByPathFinder(neighborTileColumn, neighborTileRow);

						/* Re-evaluate if there is a better path. */
						if(neighborCost < neighbor.mCost) {
							// TODO Is this ever possible with AStar ??
							if(openNodes.contains(neighbor)) {
								openNodes.remove(neighbor);
							}
							if(visitedNodes.contains(neighbor)) {
								visitedNodes.remove(neighbor);
							}
						}

						if(!openNodes.contains(neighbor) && !(visitedNodes.contains(neighbor))) {
							neighbor.mCost = neighborCost;
							if(neighbor.mCost <= pMaxCost) {
								neighbor.mExpectedRestCost = aStarHeuristic.getExpectedRestCost(tiledMap, pEntity, neighborTileColumn, neighborTileRow, pToTileColumn, pToTileRow);
								currentDepth = Math.max(currentDepth, neighbor.setParent(current));
								openNodes.add(neighbor);

								/* Ensure always the node with the lowest cost+heuristic
								 * will be used next, simply by sorting. */
								Collections.sort(openNodes);
							}
						}
					}
				}
			}
		}

		/* Check if a path was found. */
		if(toNode.mParent == null) {
			return null;
		}

		/* Traceback path. */
		final Path path = new Path();
		Node tmp = nodes[pToTileRow][pToTileColumn];
		while(tmp != fromNode) {
			path.prepend(tmp.mTileColumn, tmp.mTileRow);
			tmp = tmp.mParent;
		}
		path.prepend(pFromTileColumn, pFromTileRow);

		return path;
	}

	// ===========================================================
	// Methods
	// ===========================================================

	protected boolean isTileBlocked(final T pEntity, final int pFromTileColumn, final int pFromTileRow, final int pToTileColumn, final int pToTileRow) {
		if((pToTileColumn < 0) || (pToTileRow < 0) || (pToTileColumn >= this.mTiledMap.getTileColumns()) || (pToTileRow >= this.mTiledMap.getTileRows())) {
			return true;
		} else if((pFromTileColumn == pToTileColumn) && (pFromTileRow == pToTileRow)) {
			return true;
		}

		return this.mTiledMap.isTileBlocked(pEntity, pToTileColumn, pToTileRow);
	}

	// ===========================================================
	// Inner and Anonymous Classes
	// ===========================================================

	private static class Node implements Comparable<Node> {
		// ===========================================================
		// Constants
		// ===========================================================

		// ===========================================================
		// Fields
		// ===========================================================

		Node mParent;
		int mDepth;

		final int mTileColumn;
		final int mTileRow;

		float mCost;
		float mExpectedRestCost;

		// ===========================================================
		// Constructors
		// ===========================================================

		public Node(final int pTileColumn, final int pTileRow) {
			this.mTileColumn = pTileColumn;
			this.mTileRow = pTileRow;
		}

		// ===========================================================
		// Getter & Setter
		// ===========================================================

		public int setParent(final Node parent) {
			this.mDepth = parent.mDepth + 1;
			this.mParent = parent;

			return this.mDepth;
		}

		// ===========================================================
		// Methods for/from SuperClass/Interfaces
		// ===========================================================

		@Override
		public int compareTo(final Node pOther) {
			final float totalCost = this.mExpectedRestCost + this.mCost;
			final float totalCostOther = pOther.mExpectedRestCost + pOther.mCost;

			if (totalCost < totalCostOther) {
				return -1;
			} else if (totalCost > totalCostOther) {
				return 1;
			} else {
				return 0;
			}
		}

		// ===========================================================
		// Methods
		// ===========================================================

		// ===========================================================
		// Inner and Anonymous Classes
		// ===========================================================
	}
}
