using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace RichCreator.Utility.PathFindings
{
    public class PathGrid
    {
        private SortedDictionary<int, List<Point2>> openTree = new SortedDictionary<int, List<Point2>>();

        private HashSet<Point2> openSet = new HashSet<Point2>();
        private HashSet<Point2> closeSet = new HashSet<Point2>();
        private Dictionary<Point2, PathNode> allNodes = new Dictionary<Point2, PathNode>();

        private Point2 endPosition;
        private Point2 gridSize;

        private List<Point2> currentPath;


        /// <summary>
        /// 新建一个PathGrid，包含了网格大小和障碍物信息
        /// </summary>
        /// <param name="width"></param>
        /// <param name="height"></param>
        /// <param name="walls"></param>
        public PathGrid(int width, int height, List<Point2> walls)
        {
            gridSize = new Point2(width, height);
            for (int i = 0; i < width; i++)
            {
                for (int j = 0; j < height; j++)
                {
                    Point2 newPos = new Point2(i, j);
                    allNodes.Add(newPos, new PathNode(walls.Contains(newPos), newPos));
                }
            }
        }

        //寻路主要逻辑，通过调用该方法来获取路径信息，由一串Point2代表
        public List<Point2> FindPath(Point2 start, Point2 end)
        {
            List<Point2> result = new List<Point2>();

            this.endPosition = end;
            Point2 current = start;
            openSet.Add(current);

            while (!current.Equals(this.endPosition))
            {
                UpdatePath(current);
                if (openSet.Count == 0) return null;

                current = openTree.First().Value.First();
            }

            Point2 path = current;

            while (!path.Equals(start))
            {
                result.Add(path);
                path = allNodes[path].parent.position;
                currentPath = result;
            }

            result.Add(start);
            return result;
        }


        /// <summary>
        /// 得到路径转拆点
        /// </summary>
        /// <param name="pathPoints"></param>
        /// <returns></returns>
        public List<Point2> GetPathDiversion(List<Point2> pathPoints)
        {
            List<Point2> targets = new List<Point2>();
            if (pathPoints == null)
            {
                return targets;
            }
            Point2 last = new Point2();
            Point2 distance = new Point2();
            for (Int32 i = pathPoints.Count - 1; i >= 0; i--)
            {
                Point2 now = pathPoints[i];
                if (i == pathPoints.Count - 1)
                {
                    last = now;
                    continue;
                }
                Point2 distanceTemp = new Point2(now.x - last.x, now.y - last.y);
                if (distanceTemp != distance)
                {
                    targets.Add(last);
                    distance = distanceTemp;
                }
                last = now;
            }

            return targets;
        }

        //寻路
        private void UpdatePath(Point2 currentPos)
        {
            closeSet.Add(currentPos);
            RemoveOpen(currentPos, allNodes[currentPos]);
            List<Point2> neighborNodes = FindNeighbor(currentPos);
            foreach (Point2 nodePos in neighborNodes)
            {

                PathNode newNode = new PathNode(false, nodePos);
                newNode.parent = allNodes[currentPos];

                int g;
                int h;

                g = currentPos.x == nodePos.x || currentPos.y == nodePos.y ? 10 : 14;

                int xMoves = Math.Abs(nodePos.x - endPosition.x);
                int yMoves = Math.Abs(nodePos.y - endPosition.y);

                int min = Math.Min(xMoves, yMoves);
                int max = Math.Max(xMoves, yMoves);
                h = min * 14 + (max - min) * 10;


                newNode.gCost = g + newNode.parent.gCost;
                newNode.hCost = h;

                PathNode originNode = allNodes[nodePos];

                if (openSet.Contains(nodePos))
                {
                    if (newNode.fCost < originNode.fCost)
                    {
                        UpdateNode(newNode, originNode);
                    }
                }
                else
                {
                    allNodes[nodePos] = newNode;
                    AddOpen(nodePos, newNode);
                }
            }
        }

        //将旧节点更新为新节点
        private void UpdateNode(PathNode newNode, PathNode oldNode)
        {
            Point2 nodePos = newNode.position;
            int oldCost = oldNode.fCost;
            allNodes[nodePos] = newNode;
            List<Point2> sameCost;

            if (openTree.TryGetValue(oldCost, out sameCost))
            {
                sameCost.Remove(nodePos);
                if (sameCost.Count == 0) openTree.Remove(oldCost);
            }

            if (openTree.TryGetValue(newNode.fCost, out sameCost))
            {
                sameCost.Add(nodePos);
            }
            else
            {
                sameCost = new List<Point2> { nodePos };
                openTree.Add(newNode.fCost, sameCost);
            }
        }

        //将目标节点移出开启列表
        private void RemoveOpen(Point2 pos, PathNode node)
        {
            openSet.Remove(pos);
            List<Point2> sameCost;
            if (openTree.TryGetValue(node.fCost, out sameCost))
            {
                sameCost.Remove(pos);
                if (sameCost.Count == 0) openTree.Remove(node.fCost);
            }
        }

        //将目标节点加入开启列表
        private void AddOpen(Point2 pos, PathNode node)
        {
            openSet.Add(pos);
            List<Point2> sameCost;
            if (openTree.TryGetValue(node.fCost, out sameCost))
            {
                sameCost.Add(pos);
            }
            else
            {
                sameCost = new List<Point2> { pos };
                openTree.Add(node.fCost, sameCost);
            }
        }

        /// <summary>
        /// 找到某节点的所有相邻节点
        /// </summary>
        /// <param name="nodePos"></param>
        /// <returns></returns>
        private List<Point2> FindNeighbor(Point2 nodePos)
        {
            List<Point2> result = new List<Point2>();

            for (int x = -1; x < 2; x++)
            {
                for (int y = -1; y < 2; y++)
                {
                    if (x == 0 && y == 0) continue;

                    Point2 currentPos = new Point2(nodePos.x + x, nodePos.y + y);

                    if (currentPos.x >= gridSize.x || currentPos.y >= gridSize.y || currentPos.x < 0 || currentPos.y < 0) continue; //out of bondary
                    if (closeSet.Contains(currentPos)) continue; // already in the close list
                    if (allNodes[currentPos].isWall) continue;  // the node is a wall

                    result.Add(currentPos);
                }
            }

            return result;
        }
    }
}