//================================================================
//
// Copyright (C) 2017 Team Saluka
// All Rights Reserved
//
// Author:小妖精Balous
//
//================================================================

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

namespace Saruka
{
/// <summary>
/// A*算法
/// </summary>
public class AStar
{
private AStar() { }

/// <summary>
/// A*搜索算法
/// </summary>
/// <param name="navGrid">导航网格</param>
/// <param name="startPosition">起点坐标</param>
/// <param name="targetPosition">目标点坐标</param>
/// <param name="heuristics">启发因子</param>
/// <returns>路径</returns>
public static List<NavNode> SearchPath(NavGrid navGrid, Vector3 startPosition, Vector3 targetPosition, Heuristics heuristics)
{
if (navGrid == null)
{
Debug.LogError("你正在使用A*算法，但是没有提供导航网格！");
return null;
}

NavNode startNode = navGrid.NavNodeFromWorldPosition(startPosition);
NavNode targetNode = navGrid.NavNodeFromWorldPosition(targetPosition);

if (!targetNode.isWalkable) return null;

List<NavNode> queueNodes = new List<NavNode>();
HashSet<NavNode> evaluatedNodes = new HashSet<NavNode>();

queueNodes.Add(startNode);

while(queueNodes.Count > 0)
{
NavNode currentNode = queueNodes[0];
for (int i = 1; i < queueNodes.Count; i++)
if (queueNodes[i].fCost < currentNode.fCost || queueNodes[i].fCost == currentNode.fCost && queueNodes[i].hCost < currentNode.hCost)
currentNode = queueNodes[i];

queueNodes.Remove(currentNode);
evaluatedNodes.Add(currentNode);

//找到路径，返回路径
if (currentNode == targetNode)
{
List<NavNode> path = new List<NavNode>();
NavNode node = targetNode;
while(node != startNode)
{
path.Add(node);
node = node.parent;
}
path.Reverse();
return path;
}

foreach (NavNode neighborNode in navGrid.GetNeighborNodes(currentNode))
{
if (!neighborNode.isWalkable || evaluatedNodes.Contains(neighborNode)) continue;

float newGCostToNeighborNode = currentNode.gCost + heuristics.GetHeuristics(currentNode, neighborNode);
if (!queueNodes.Contains(neighborNode) || newGCostToNeighborNode < neighborNode.gCost)
{
if (!queueNodes.Contains(neighborNode)) queueNodes.Add(neighborNode);
neighborNode.gCost = newGCostToNeighborNode;
neighborNode.hCost = heuristics.GetHeuristics(neighborNode, targetNode);
neighborNode.parent = currentNode;
}
}
}
//找不到路径，返回null
return null;
}
}
}