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

public class Pathfinder {

	public static List<MapTile> FindPathDijkstra(MapTile[,] map, Map.Vector2Int startPoint, Map.Vector2Int endPoint)
	{
		MapTile startNode = map [startPoint.X, startPoint.Y];
		MapTile targetNode = map [endPoint.X, endPoint.Y];

		HashSet<MapTile> toVisit = new HashSet<MapTile> ();

		foreach (MapTile tile in map) {
			tile.GCost = int.MaxValue;
			tile.TileParent = null;
			toVisit.Add (tile);
		}

		startNode.GCost = 0;

		while (toVisit.Count > 0) {

			int minimal = toVisit.Min (x => x.GCost);
			MapTile currentTile = toVisit.Where (x => x.GCost == minimal).FirstOrDefault ();
			toVisit.Remove (currentTile);

			if (currentTile == targetNode) {
				return RetracePath(startNode,targetNode);
			}

			foreach (MapTile neighbour in MapManager.GetNeighbours(map, currentTile.Coordinates)) {

				if (neighbour.IsObstacle)
					continue;

				int newCostToNeighbour = currentTile.GCost + GetTilesDistance(currentTile, neighbour);
				if (newCostToNeighbour < neighbour.GCost) {
					neighbour.GCost = newCostToNeighbour;
					neighbour.TileParent = currentTile;
				}
			}
		}
		return null;
	}

	public static List<MapTile> FindPathAStar(MapTile[,] map, Map.Vector2Int startPoint, Map.Vector2Int endPoint)
	{
		MapTile startNode = map [startPoint.X, startPoint.Y];
		MapTile targetNode = map [endPoint.X, endPoint.Y];

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

		while (openSet.Count > 0) {
			MapTile tile = openSet[0];
			for (int i = 1; i < openSet.Count; i ++) {
				if (openSet[i].fCost < tile.fCost || openSet[i].fCost == tile.fCost) {
					if (openSet[i].HCost < tile.HCost)
						tile = openSet[i];
				}
			}

			openSet.Remove(tile);
			closedSet.Add(tile);

			if (tile == targetNode) {
				return RetracePath(startNode,targetNode);
			}

			foreach (MapTile neighbour in MapManager.GetNeighbours(map, tile.Coordinates)) {
				if (neighbour.IsObstacle || closedSet.Contains(neighbour)) {
					continue;
				}

				int newCostToNeighbour = tile.GCost + GetTilesDistance(tile, neighbour);
				if (newCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour)) {
					neighbour.GCost = newCostToNeighbour;
					neighbour.HCost = GetTilesDistance(neighbour, targetNode);
					neighbour.TileParent = tile;

					if (!openSet.Contains(neighbour))
						openSet.Add(neighbour);
				}
			}
		}
		return null;
	}

	private static int GetTilesDistance(MapTile tileA, MapTile tileB) {
		float dstX = Mathf.Abs(tileA.Coordinates.X - tileB.Coordinates.X);
		float dstY = Mathf.Abs(tileA.Coordinates.Y - tileB.Coordinates.Y);
		return (int)(dstX + dstY);
	}

	private static List<MapTile> RetracePath(MapTile startNode, MapTile endNode) {
		List<MapTile> path = new List<MapTile>();
		MapTile currentNode = endNode;

		while (currentNode != startNode) {
			path.Add(currentNode);
			if (currentNode == null)
				return null;

			currentNode = currentNode.TileParent;
		}
		path.Reverse();

		return path;
	}
}
