Pathfinding

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;

public class Pathfinding : MonoBehaviour {

	PathRequestManager rmInstance;
	Grid grid;

	void Awake() {
		rmInstance = GetComponent<PathRequestManager> ();
		grid = GetComponent<Grid> ();
	}

	public void StartFindPath(Vector3 startPos, Vector3 targetPos) {
		StartCoroutine(FindPath(startPos,targetPos));
	}

	IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) {
		Vector3[] waypoints = new Vector3[0];
		bool pathSuccess = false;

		Node startNode = grid.NodeFromWorldPoint(startPos);
		Node targetNode = grid.NodeFromWorldPoint(targetPos);

		if ((startNode.walkable && startNode.unoccupied) && (targetNode.walkable && targetNode.unoccupied)) {

			List<Node> openSet = new List<Node> ();
			HashSet<Node> closedSet = new HashSet<Node> ();
			openSet.Add (startNode);

			// current node = node in OPEN set with lowest f_cost
			while (openSet.Count > 0) {
				Node node = openSet [0];
				for (int i = 1; i < openSet.Count; i++) {
					if (openSet [i].fCost < node.fCost || openSet [i].fCost == node.fCost) {
						if (openSet [i].hCost < node.hCost)
							node = openSet [i];
					}
				}

				//Remove the last node with a lower f_cost from open set to closed set
				openSet.Remove (node);
				closedSet.Add (node);

				//Path has been found
				if (node == targetNode) {
					pathSuccess = true;
					break;
				}

				//Foreach neighbour of the current node
				foreach (Node neighbour in grid.GetNeighbours(node)) {
					//if the neighbour is not traversable or is in closed set
					if (!neighbour.walkable || !neighbour.unoccupied|| closedSet.Contains (neighbour)) {
						//skip to next neighbour
						continue;
					}

					int newCostToNeighbour = node.gCost + GetDistance (node, neighbour);
					if (newCostToNeighbour < neighbour.gCost || !openSet.Contains (neighbour)) {
						//Set f_cost of neighbour
						neighbour.gCost = newCostToNeighbour;
						neighbour.hCost = GetDistance (neighbour, targetNode);
						//Set parent of neighbour to current
						neighbour.parent = node;

						//If neighbour is not in open set, add neighbour to open set
						if (!openSet.Contains (neighbour))
							openSet.Add (neighbour);
					}
				}
			}
		}
		yield return null;
		if (pathSuccess) {
			waypoints = RetracePath(startNode,targetNode);
		}
		rmInstance.FinishedProcessingPath (waypoints, pathSuccess);
	}

	Vector3[] RetracePath(Node startNode, Node endNode) {
		List<Node> path = new List<Node>();
		Node currentNode = endNode;

		while (currentNode != startNode) {
			path.Add(currentNode);
			currentNode = currentNode.parent;
		}
		Vector3[] waypoints = SimplifyPath(path);
		Array.Reverse (waypoints);
		return waypoints;

	}

	//Simplify Path by removing wp until change in direction
	Vector3[] SimplifyPath(List<Node> path) {
		List<Vector3> waypoints = new List<Vector3> ();
		Vector2 directionOld = Vector2.zero;

		for (int i = 1; i < path.Count; i++) {
			Vector2 directionNew = new Vector2 (path [i - 1].gridX - path [i].gridX, path [i - 1].gridY - path [i].gridY);
			if (directionNew != directionOld) {
					waypoints.Add(path[i-1].worldPosition);
			}
			directionOld = directionNew;
		}
		return waypoints.ToArray ();
	}

	int GetDistance(Node nodeA, Node nodeB) {
		int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
		int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);

		if (dstX > dstY)
			return 14*dstY + 10* (dstX-dstY);
		return 14*dstX + 10 * (dstY-dstX);
	}
}