using System;
using System.Collections.Generic;
using static HxGame.PathFinding.IPathFinder;

namespace HxGame.PathFinding
{
    internal struct PathFinderNodeFast
	{
		public float F;
		// f = gone + heuristic
		public float G;
		public ushort PX;
		// Parent
		public ushort PY;
		public byte Status;
		public int Steps;
	}

	public class PathFinderFast : IPathFinder
    {
		private CellNode[] mGrid = null;	//所有节点
		private PriorityQueueB<int> mOpen = null;
		private List<PathFinderNode> mClose = new List<PathFinderNode>();
		private HeuristicFormula mFormula = HeuristicFormula.Manhattan;		//消耗类型
		private bool mDiagonals = true;			//是否能斜起走
		private bool mHexagonalGrid = false;	//是否是六边形格子
		private float mHEstimate = 1;			//消耗预估
		private float mHeavyDiagonalsCost = 1.4f;	//斜边消耗
		private int mMaxSteps = 2000;			//最大步数
		private float mMaxSearchCost = 100000;	//最大寻路消耗
		private PathFinderNodeFast[] mCalcGrid = null;
		private byte mOpenNodeValue = 1;
		private byte mCloseNodeValue = 2;
		private OnCellCross mOnCellCross = null;

		private float mH = 0;
		private int mLocation = 0;			//起点的索引
		private int mNewLocation = 0;		//终点的索引
		private ushort mLocationX = 0;		//起点的行索引
		private ushort mLocationY = 0;		//起点的列索引
		private ushort mNewLocationX = 0;   //计算过程中的行索引
		private ushort mNewLocationY = 0;   //计算过程中的列索引
		private ushort mGridX = 0;			//最大行数
		private ushort mGridY = 0;			//最大列数
		private ushort mGridXMinus1 = 0;	//最大行-1
		private ushort mGridYLog2 = 0;		//最大列2的幂
		private bool mFound = false;

		private sbyte[,] mDirection = new sbyte[8, 2]
		{
			{ 0, -1},	//bottom
			{ 1, 0},	//right
			{ 0, 1},	//top
			{ -1, 0},	//left
			{ 1, -1},	//right-bottom 
			{ 1, 1},	//right-top
			{ -1, 1},	//left-top
			{ -1, -1},	//left-bottom 
		};
		private readonly sbyte[,] mDirectionHex0 = new sbyte[6, 2]
		{
			{ 0, -1 },	//top
			{ 1, 0 },	//right
			{ 0, 1 },	//bottom
			{ -1, 0 },	//left
			{ 1, 1},	//right-bottom
			{ -1,1 }	//left-bottom
		};
		private readonly sbyte[,] mDirectionHex1 = new sbyte[6, 2]
		{
			{ 0, -1 },	//top
			{ 1, 0 },	//right
			{ 0, 1 },	//bottom
			{ -1, 0 },  //left
			{ -1, -1 },	//left-top
			{ 1, -1}	//right-top
		};
		private readonly int[] mCellSide0 = new int[6] {
			(int)CELL_SIDE.Bottom,
			(int)CELL_SIDE.BottomRight,
			(int)CELL_SIDE.Top,
			(int)CELL_SIDE.BottomLeft,
			(int)CELL_SIDE.TopRight,
			(int)CELL_SIDE.TopLeft
		};
		private readonly int[] mCellSide1 = new int[6] {
			(int)CELL_SIDE.Bottom,
			(int)CELL_SIDE.TopRight,
			(int)CELL_SIDE.Top,
			(int)CELL_SIDE.TopLeft,
			(int)CELL_SIDE.BottomLeft,
			(int)CELL_SIDE.BottomRight
		};
		private readonly int[] mCellBoxSides = new int[8] {
			(int)CELL_SIDE.Bottom,
			(int)CELL_SIDE.Right,
			(int)CELL_SIDE.Top,
			(int)CELL_SIDE.Left,
			(int)CELL_SIDE.BottomRight,
			(int)CELL_SIDE.TopRight,
			(int)CELL_SIDE.TopLeft,
			(int)CELL_SIDE.BottomLeft
		};

		private int mEndLocation = 0;   //终点的索引
		private float mNewG = 0;		//计算过程中的g值
		private int mCellGroupMask = -1;
		private bool mIgnoreCanCrossCheck;		//忽略阻挡
		private bool mIgnoreCellCost;			//忽略格子消耗
		private bool mIncludeInvisibleCells;

		public void SetCalcMatrix(CellNode[] grid)
		{
			if (grid == null)
				throw new Exception("Grid cannot be null");
			if (grid.Length != mGrid.Length) 
				throw new Exception("SetCalcMatrix called with matrix with different dimensions. Call constructor instead.");
			mGrid = grid;

			Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
			ComparePFNodeMatrix comparer = (ComparePFNodeMatrix)mOpen.comparer;
			comparer.SetMatrix(mCalcGrid);
		}

		public HeuristicFormula Formula
		{
			get { return mFormula; }
			set { mFormula = value; }
		}

		public bool Diagonals
		{
			get { return mDiagonals; }
			set
			{
				mDiagonals = value;
				if (mDiagonals)
					mDirection = new sbyte[8, 2] {
						{ 0, -1 },
						{ 1, 0 },
						{ 0, 1 },
						{ -1, 0 },
						{ 1, -1 },
						{ 1, 1 },
						{ -1, 1 },
						{ -1, -1 }
					};
				else
					mDirection = new sbyte[4, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };
			}
		}

		public float HeavyDiagonalsCost
		{
			get { return mHeavyDiagonalsCost; }
			set { mHeavyDiagonalsCost = value; }
		}

		public bool HexagonalGrid
		{
			get { return mHexagonalGrid; }
			set { mHexagonalGrid = value; }
		}

		public float HeuristicEstimate
		{
			get { return mHEstimate; }
			set { mHEstimate = value; }
		}

		public float MaxSearchCost
		{
			get { return mMaxSearchCost; }
			set { mMaxSearchCost = value; }
		}

		public int MaxSteps
		{
			get { return mMaxSteps; }
			set { mMaxSteps = value; }
		}

		public int CellGroupMask
		{
			get { return mCellGroupMask; }
			set { mCellGroupMask = value; }
		}

		public bool IgnoreCanCrossCheck
		{
			get { return mIgnoreCanCrossCheck; }
			set { mIgnoreCanCrossCheck = value; }
		}

		public bool IgnoreCellCost
		{
			get { return mIgnoreCellCost; }
			set { mIgnoreCellCost = value; }
		}

		public bool IncludeInvisibleCells
		{
			get { return mIncludeInvisibleCells; }
			set { mIncludeInvisibleCells = value; }
		}

		public OnCellCross OnCellCross
		{
			get { return mOnCellCross; }
			set { mOnCellCross = value; }
		}

		public PathFinderFast(CellNode[] grid, int gridWidth, int gridHeight)
		{
			if (grid == null)
				throw new Exception("Grid cannot be null");

			mGrid = grid;
			mGridX = (ushort)gridWidth;
			mGridY = (ushort)gridHeight;
			mGridXMinus1 = (ushort)(mGridX - 1);
			mGridYLog2 = (ushort)Math.Log(mGridX, 2);

			// This should be done at the constructor, for now we leave it here.
			if (Math.Log(mGridX, 2) != (int)Math.Log(mGridX, 2))
				throw new Exception("Invalid Grid, size in X must be power of 2");

			if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY))
				mCalcGrid = new PathFinderNodeFast[mGridX * mGridY];

			mOpen = new PriorityQueueB<int>(new ComparePFNodeMatrix(mCalcGrid));
		}

		public List<PathFinderNode> FindPath(CellNode startCell, CellNode endCell, out float totalCost, bool evenLayout)
        {
			totalCost = 0;
			mFound = false;
			int evenLayoutValue = evenLayout ? 1 : 0;
			if (mOpenNodeValue > 250)
			{
				Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
				mOpenNodeValue = 1;
				mCloseNodeValue = 2;
			}
			else
			{
				mOpenNodeValue += 2;
				mCloseNodeValue += 2;
			}
			mOpen.Clear();
			mClose.Clear();

			int maxi;	//周边格子数
			if (mHexagonalGrid)
				maxi = 6;
			else
				maxi = mDiagonals ? 8 : 4;

			//mLocation = (startCell.row << mGridYLog2) + startCell.column;
			//mEndLocation = (endCell.row << mGridYLog2) + endCell.column;
			mLocation = (startCell.Y << mGridYLog2) + startCell.X;
			mEndLocation = (endCell.Y << mGridYLog2) + endCell.X;
			mCalcGrid[mLocation].G = 0;
			mCalcGrid[mLocation].F = mHEstimate;
			//mCalcGrid[mLocation].PX = (ushort)startCell.column;
			//mCalcGrid[mLocation].PY = (ushort)startCell.row;
			mCalcGrid[mLocation].PX = (ushort)startCell.X;
			mCalcGrid[mLocation].PY = (ushort)startCell.Y;
			mCalcGrid[mLocation].Status = mOpenNodeValue;
			mCalcGrid[mLocation].Steps = 0;

			mOpen.Push(mLocation);
			while (mOpen.Count > 0)
            {
				mLocation = mOpen.Pop();

				//是否在封闭列表中？表示已处理此节点
				if (mCalcGrid[mLocation].Status == mCloseNodeValue)
					continue;

				if (mLocation == mEndLocation)
				{
					mCalcGrid[mLocation].Status = mCloseNodeValue;
					mFound = true;
					break;
				}

				mLocationX = (ushort)(mLocation & mGridXMinus1);
				mLocationY = (ushort)(mLocation >> mGridYLog2);

				//计算周边每个格子
				bool hasSideCosts = false;
				float[] sideCosts = mGrid[mLocation].crossCost;
				if (!mIgnoreCellCost && sideCosts != null)
					hasSideCosts = true;

				for (int i = 0; i < maxi; i++)
                {
					int cellSide;
					if (mHexagonalGrid)
					{
						if (mLocationX % 2 == evenLayoutValue)
						{
							mNewLocationX = (ushort)(mLocationX + mDirectionHex0[i, 0]);
							mNewLocationY = (ushort)(mLocationY + mDirectionHex0[i, 1]);
							cellSide = mCellSide0[i];
						}
						else
						{
							mNewLocationX = (ushort)(mLocationX + mDirectionHex1[i, 0]);
							mNewLocationY = (ushort)(mLocationY + mDirectionHex1[i, 1]);
							cellSide = mCellSide1[i];
						}
					}
					else
					{
						mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]);
						mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]);
						cellSide = mCellBoxSides[i];
					}

					if (mNewLocationY >= mGridY || mNewLocationX >= mGridX)
						continue;

					mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX;
					CellNode nextCell = mGrid[mNewLocation];
					if (nextCell == null || (nextCell.cellType == MapManager.CellType.Obstacle  && !mIgnoreCanCrossCheck))
						continue;

					//if (!mIncludeInvisibleCells && !mGrid[mNewLocation].visible)
					//	continue;

					float gridValue = (nextCell.group & mCellGroupMask) != 0 ? 1 : 0;
					if (gridValue == 0)
						continue;

					if (hasSideCosts)
					{
						gridValue = sideCosts[cellSide];
						if (gridValue <= 0)
							gridValue = 1;
					}

					if (mOnCellCross != null)
						gridValue += mOnCellCross(mNewLocation);

					if (!mHexagonalGrid && i > 3)
						mNewG = mCalcGrid[mLocation].G + gridValue * mHeavyDiagonalsCost;
					else
						mNewG = mCalcGrid[mLocation].G + gridValue;

					if (mNewG > mMaxSearchCost || mCalcGrid[mLocation].Steps >= mMaxSteps)
						continue;

					if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue)
					{
						if (mCalcGrid[mNewLocation].G <= mNewG)
							continue;
					}

					mCalcGrid[mNewLocation].PX = mLocationX;
					mCalcGrid[mNewLocation].PY = mLocationY;
					mCalcGrid[mNewLocation].G = mNewG;
					mCalcGrid[mNewLocation].Steps = mCalcGrid[mLocation].Steps + 1;

					//int dist = Math.Abs(mNewLocationX - endCell.column);
					int dist = Math.Abs(mNewLocationX - endCell.X);

					switch (mFormula)
					{
						default:
						case HeuristicFormula.Manhattan:
							//mH = mHEstimate * (dist + Math.Abs(mNewLocationY - endCell.row));
							mH = mHEstimate * (dist + Math.Abs(mNewLocationY - endCell.Y));
							break;
						case HeuristicFormula.MaxDXDY:
							//mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - endCell.row)));
							mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - endCell.Y)));
							break;
						case HeuristicFormula.DiagonalShortCut:
							//float h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - endCell.row));
							//float h_straight = (dist + Math.Abs(mNewLocationY - endCell.row));
							float h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - endCell.Y));
							float h_straight = (dist + Math.Abs(mNewLocationY - endCell.Y));
							mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
							break;
						case HeuristicFormula.Euclidean:
							//mH = mHEstimate * (float)(Math.Sqrt(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - endCell.row), 2)));
							mH = mHEstimate * (float)(Math.Sqrt(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - endCell.Y), 2)));
							break;
						case HeuristicFormula.EuclideanNoSQR:
							//mH = mHEstimate * (float)(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - endCell.row), 2));
							mH = mHEstimate * (float)(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - endCell.Y), 2));
							break;
					}
					mCalcGrid[mNewLocation].F = mNewG + mH;

					mOpen.Push(mNewLocation);
					mCalcGrid[mNewLocation].Status = mOpenNodeValue;
				}
				mCalcGrid[mLocation].Status = mCloseNodeValue;
			}

			if (mFound)
			{
				mClose.Clear();
				//int posX = endCell.column;
				//int posY = endCell.row;
				int posX = endCell.X;
				int posY = endCell.Y;

				//PathFinderNodeFast fNodeTmp = mCalcGrid[(endCell.row << mGridYLog2) + endCell.column];
				PathFinderNodeFast fNodeTmp = mCalcGrid[(endCell.Y << mGridYLog2) + endCell.X];
				totalCost = fNodeTmp.G;
				PathFinderNode fNode;
				fNode.F = fNodeTmp.F;
				fNode.G = fNodeTmp.G;
				fNode.H = 0;
				fNode.PX = fNodeTmp.PX;
				fNode.PY = fNodeTmp.PY;
				//fNode.X = endCell.column;
				//fNode.Y = endCell.row;
				fNode.X = endCell.X;
				fNode.Y = endCell.Y;

				while (fNode.X != fNode.PX || fNode.Y != fNode.PY)
				{
					mClose.Add(fNode);
					posX = fNode.PX;
					posY = fNode.PY;
					fNodeTmp = mCalcGrid[(posY << mGridYLog2) + posX];
					fNode.F = fNodeTmp.F;
					fNode.G = fNodeTmp.G;
					fNode.H = 0;
					fNode.PX = fNodeTmp.PX;
					fNode.PY = fNodeTmp.PY;
					fNode.X = posX;
					fNode.Y = posY;
				}
				return mClose;
			}
			return null;
		}









		internal class ComparePFNodeMatrix : IComparer<int>
		{
			protected PathFinderNodeFast[] mMatrix;

			public ComparePFNodeMatrix(PathFinderNodeFast[] matrix)
			{
				mMatrix = matrix;
			}

			public int Compare(int a, int b)
			{
				if (mMatrix[a].F > mMatrix[b].F)
					return 1;
				else if (mMatrix[a].F < mMatrix[b].F)
					return -1;
				return 0;
			}

			public void SetMatrix(PathFinderNodeFast[] matrix)
			{
				mMatrix = matrix;
			}
		}
	}
}