//
//  THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
//  KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
//  IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
//  PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER 
//  REMAINS UNCHANGED.
//
//  Email:  gustavo_franco@hotmail.com
//
//  Copyright (C) 2006 Franco, Gustavo 
//
//  Some modifications by Kronnect to reuse grid buffers between calls and to allow different grid configurations in same grid array (uses bitwise differentiator)
//  Also including support for hexagonal grids and some other improvements

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

namespace TGS.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 {

		// Heap variables are initializated to default, but I like to do it anyway
		private Cell[] mGrid;
		private PriorityQueueB<int> mOpen;
		private List<PathFinderNode> mClose = new List<PathFinderNode> ();
		private HeuristicFormula mFormula = HeuristicFormula.Manhattan;
		private bool mDiagonals = true;
		private CellType mCellShape;
		private float mHEstimate = 1;
		private float mHeavyDiagonalsCost = 1.4f;
		private int mMaxSteps = 2000;
		private float mMaxSearchCost = 100000;
		private float mMaxCellCrossCost;
		private PathFinderNodeFast[] mCalcGrid;
		private byte mOpenNodeValue = 1;
		private byte mCloseNodeValue = 2;
		private OnCellCross mOnCellCross;
        private int mMinClearance;

        //Promoted local variables to member variables to avoid recreation between calls
        private float mH;
		private int mLocation;
		private int mNewLocation;
		private ushort mLocationX;
		private ushort mLocationY;
		private ushort mNewLocationX;
		private ushort mNewLocationY;
		private ushort mColumnCount;
		private ushort mRowCount;
		private ushort mColumnCountMinus1;
		private ushort mRowCountLog2;
		private bool mFound = false;
  
        private sbyte[,] mDirectionBox = new sbyte[8, 2] {
            { 0, -1 },
            { 1, 0 },
            { 0, 1 },
            { -1, 0 },
            { 1, -1 },
            { 1, 1 },
            { -1, 1 },
            { -1, -1 }
        };
        private readonly sbyte[,] mDirectionFlatHex0 = new sbyte[6, 2] {
            { 0, -1 },
            { 1, 0 },
            { 0, 1 },
            { -1, 0 },
            { 1, 1 },
            { -1, 1 }
        };
        private readonly sbyte[,] mDirectionFlatHex1 = new sbyte[6, 2] {
            { 0, -1 },
            { 1, 0 },
            { 0, 1 },
            { -1, 0 },
            { -1, -1 },
            { 1, -1 }
        };
        private readonly int[] mCellFlatSide0 = 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[] mCellFlatSide1 = 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 sbyte[,] mDirectionPointyHex0 = new sbyte[6, 2] {
            { 0, -1 },
            { -1, 0 },
            { 0, 1 },
            { 1, 1 },
            { 1, 0 },
            { 1, -1 }
        };
        private readonly sbyte[,] mDirectionPointyHex1 = new sbyte[6, 2] {
            { -1, -1 },
            { -1, 0 },
            { -1, 1 },
            { 0, 1 },
            { 1, 0 },
            { 0, -1 }
        };
        private readonly int[] mCellPointySide0 = new int[6] {
            (int)CELL_SIDE.BottomLeft,
            (int)CELL_SIDE.Left,
            (int)CELL_SIDE.TopLeft,
            (int)CELL_SIDE.TopRight,
            (int)CELL_SIDE.Right,
            (int)CELL_SIDE.BottomRight
        };
        private readonly int[] mCellPointySide1 = new int[6] {
            (int)CELL_SIDE.BottomLeft,
            (int)CELL_SIDE.Left,
            (int)CELL_SIDE.TopLeft,
            (int)CELL_SIDE.TopRight,
            (int)CELL_SIDE.Right,
            (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;
		private int mCellGroupMask = -1;
		private bool mIgnoreCanCrossCheck;
		private bool mIgnoreCellCost;
		private bool mIncludeInvisibleCells;

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

			mGrid = grid;
			mColumnCount = (ushort)gridWidth;
			mRowCount = (ushort)gridHeight;
			mColumnCountMinus1 = (ushort)(mColumnCount - 1);
			mRowCountLog2 = (ushort)Math.Log (mColumnCount, 2);

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

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

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

		public void SetCalcMatrix (Cell[] grid) {
			if (grid == null)
				throw new Exception ("Grid cannot be null");
			if (grid.Length != mGrid.Length) // mGridX != (ushort) (mGrid.GetUpperBound(0) + 1) || mGridY != (ushort) (mGrid.GetUpperBound(1) + 1))
				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)
					mDirectionBox = new sbyte[8, 2] {
						{ 0, -1 },
						{ 1, 0 },
						{ 0, 1 },
						{ -1, 0 },
						{ 1, -1 },
						{ 1, 1 },
						{ -1, 1 },
						{ -1, -1 }
					};
				else
                    mDirectionBox = new sbyte[4, 2]{ { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };
			}
		}

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

		public CellType CellShape {
			get { return mCellShape; }
			set { mCellShape = value; }
		}

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

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

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


		public OnCellCross OnCellCross {
			get { return mOnCellCross; }
			set { mOnCellCross = 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 int MinClearance {
            get { return mMinClearance; }
            set { mMinClearance = value; }
        }

		public int RowCount {
			get { return mRowCount; }
			set { mRowCount = (ushort)value; }
		}

        public int ColumnCount {
            get { return mColumnCount; }
            set { mColumnCount = (ushort)value; }
        }


        public List<PathFinderNode> FindPath (TerrainGridSystem tgs, Cell startCell, Cell 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;
			bool isHexagonal = mCellShape == CellType.FlatTopHexagon || mCellShape == CellType.PointyTopHexagon;
			if (isHexagonal) {
				maxi = 6;
			} else {
				maxi = mDiagonals ? 8 : 4;
			}
  
			mLocation = (startCell.row << mRowCountLog2) + startCell.column;
			mEndLocation = (endCell.row << mRowCountLog2) + endCell.column;
			mCalcGrid [mLocation].G = 0;
			mCalcGrid [mLocation].F = mHEstimate;
			mCalcGrid [mLocation].PX = (ushort)startCell.column;
			mCalcGrid [mLocation].PY = (ushort)startCell.row;
			mCalcGrid [mLocation].Status = mOpenNodeValue;
			mCalcGrid [mLocation].Steps = 0;
  
			mOpen.Push (mLocation);
			while (mOpen.Count > 0) {
				mLocation = mOpen.Pop ();
  
				//Is it in closed list? means this node was already processed
				if (mCalcGrid [mLocation].Status == mCloseNodeValue)
					continue;
  
				if (mLocation == mEndLocation) {
					mCalcGrid [mLocation].Status = mCloseNodeValue;
					mFound = true;
					break;
				}
  
				mLocationX = (ushort)(mLocation & mColumnCountMinus1);
				mLocationY = (ushort)(mLocation >> mRowCountLog2);
  
				//Lets calculate each successors
				bool hasSideCosts = false;
                float[] sideCosts = mGrid[mLocation].crossCost;
                if (!mIgnoreCellCost && sideCosts != null) {
					hasSideCosts = true;
				}
  
				for (int i = 0; i < maxi; i++) {
  
                    int cellSide;
                    if (mCellShape == CellType.FlatTopHexagon)
                    {
                        if (mLocationX % 2 == evenLayoutValue)
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionFlatHex0[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionFlatHex0[i, 1]);
                            cellSide = mCellFlatSide0[i];
                        }
                        else
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionFlatHex1[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionFlatHex1[i, 1]);
                            cellSide = mCellFlatSide1[i];
                        }
                    }
                    else if (mCellShape == CellType.PointyTopHexagon)
                    {
                        if (mLocationY % 2 == evenLayoutValue)
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionPointyHex0[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionPointyHex0[i, 1]);
                            cellSide = mCellPointySide0[i];
                        }
                        else
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionPointyHex1[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionPointyHex1[i, 1]);
                            cellSide = mCellPointySide1[i];
                        }
  
                    }
                    else { 
                        mNewLocationX = (ushort)(mLocationX + mDirectionBox[i, 0]);
						mNewLocationY = (ushort)(mLocationY + mDirectionBox[i, 1]);
						cellSide = mCellBoxSides [i];
					}
  
					if (mNewLocationY >= mRowCount || mNewLocationX >= mColumnCount)
						continue;
  
					// Unbreakeable?
					mNewLocation = (mNewLocationY << mRowCountLog2) + mNewLocationX;
                    Cell nextCell = mGrid[mNewLocation];
					if (nextCell == null || (!nextCell.canCross && !mIgnoreCanCrossCheck))
						continue;
  
                    if (!mIncludeInvisibleCells && !mGrid[mNewLocation].visible)
						continue;
  
                    if (nextCell.clearance < mMinClearance) {
                        continue;
                    }
  
                    float gridValue = (nextCell.group & mCellGroupMask) != 0 ? 1 : 0;
					if (gridValue == 0)
						continue;
  
					if (hasSideCosts) {
						gridValue = sideCosts [cellSide];
						if (gridValue > mMaxCellCrossCost) continue;
						if (gridValue <= 0)
							gridValue = 1;
					}
  
					// Check custom validator
					if (mOnCellCross != null) {
						gridValue += mOnCellCross (tgs, mNewLocation);
					}
  
					if (!isHexagonal && i > 3)
						mNewG = mCalcGrid [mLocation].G + gridValue * mHeavyDiagonalsCost;
					else
						mNewG = mCalcGrid [mLocation].G + gridValue;
  
					if (mNewG > mMaxSearchCost || mCalcGrid [mLocation].Steps >= mMaxSteps)
						continue;
  
					//Is it open or closed?
					if (mCalcGrid [mNewLocation].Status == mOpenNodeValue || mCalcGrid [mNewLocation].Status == mCloseNodeValue) {
						// The current node has less code than the previous? then skip this node
						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);
					switch (mFormula) {
					default:
					case HeuristicFormula.Manhattan:
						mH = mHEstimate * (dist + Math.Abs (mNewLocationY - endCell.row));
						break;
					case HeuristicFormula.MaxDXDY:
						mH = mHEstimate * (Math.Max (dist, Math.Abs (mNewLocationY - endCell.row)));
						break;
					case HeuristicFormula.DiagonalShortCut:
						float h_diagonal = Math.Min (dist, Math.Abs (mNewLocationY - endCell.row));
						float h_straight = (dist + Math.Abs (mNewLocationY - endCell.row));
						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)));
						break;
					case HeuristicFormula.EuclideanNoSQR:
						mH = mHEstimate * (float)(Math.Pow (dist, 2) + Math.Pow ((mNewLocationY - endCell.row), 2));
						break;
					case HeuristicFormula.Custom1:
						PathFindingPoint dxy = new PathFindingPoint (dist, Math.Abs (endCell.row - mNewLocationY));
						float Orthogonal = Math.Abs (dxy.x - dxy.y);
						float Diagonal = Math.Abs (((dxy.x + dxy.y) - Orthogonal) / 2);
						mH = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
						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;
  
				PathFinderNodeFast fNodeTmp = mCalcGrid [(endCell.row << mRowCountLog2) + endCell.column];
				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;
  
				while (fNode.X != fNode.PX || fNode.Y != fNode.PY) {
					mClose.Add (fNode);
					posX = fNode.PX;
					posY = fNode.PY;
					fNodeTmp = mCalcGrid [(posY << mRowCountLog2) + 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;
		}
public List<PathFinderNode> FindPath (TerrainGridSystem tgs, Cell startCell, Cell endCell, int crossLevel, 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;
			bool isHexagonal = mCellShape == CellType.FlatTopHexagon || mCellShape == CellType.PointyTopHexagon;
			if (isHexagonal) {
				maxi = 6;
			} else {
				maxi = mDiagonals ? 8 : 4;
			}

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

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

				//Is it in closed list? means this node was already processed
				if (mCalcGrid [mLocation].Status == mCloseNodeValue)
					continue;

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

				mLocationX = (ushort)(mLocation & mColumnCountMinus1);
				mLocationY = (ushort)(mLocation >> mRowCountLog2);

				//Lets calculate each successors
				bool hasSideCosts = false;
                float[] sideCosts = mGrid[mLocation].crossCost;
                if (!mIgnoreCellCost && sideCosts != null) {
					hasSideCosts = true;
				}

				for (int i = 0; i < maxi; i++) {

                    int cellSide;
                    if (mCellShape == CellType.FlatTopHexagon)
                    {
                        if (mLocationX % 2 == evenLayoutValue)
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionFlatHex0[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionFlatHex0[i, 1]);
                            cellSide = mCellFlatSide0[i];
                        }
                        else
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionFlatHex1[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionFlatHex1[i, 1]);
                            cellSide = mCellFlatSide1[i];
                        }
                    }
                    else if (mCellShape == CellType.PointyTopHexagon)
                    {
                        if (mLocationY % 2 == evenLayoutValue)
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionPointyHex0[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionPointyHex0[i, 1]);
                            cellSide = mCellPointySide0[i];
                        }
                        else
                        {
                            mNewLocationX = (ushort)(mLocationX + mDirectionPointyHex1[i, 0]);
                            mNewLocationY = (ushort)(mLocationY + mDirectionPointyHex1[i, 1]);
                            cellSide = mCellPointySide1[i];
                        }

                    }
                    else { 
                        mNewLocationX = (ushort)(mLocationX + mDirectionBox[i, 0]);
						mNewLocationY = (ushort)(mLocationY + mDirectionBox[i, 1]);
						cellSide = mCellBoxSides [i];
					}

					if (mNewLocationY >= mRowCount || mNewLocationX >= mColumnCount)
						continue;

					// Unbreakeable?
					mNewLocation = (mNewLocationY << mRowCountLog2) + mNewLocationX;
                    Cell nextCell = mGrid[mNewLocation];
					if (nextCell == null || (!nextCell.CanCross(crossLevel) && !mIgnoreCanCrossCheck))
						continue;

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

                    if (nextCell.clearance < mMinClearance) {
                        continue;
                    }

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

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

					// Check custom validator
					if (mOnCellCross != null) {
						gridValue += mOnCellCross (tgs, mNewLocation);
					}

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

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

					//Is it open or closed?
					if (mCalcGrid [mNewLocation].Status == mOpenNodeValue || mCalcGrid [mNewLocation].Status == mCloseNodeValue) {
						// The current node has less code than the previous? then skip this node
						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);
					switch (mFormula) {
					default:
					case HeuristicFormula.Manhattan:
						mH = mHEstimate * (dist + Math.Abs (mNewLocationY - endCell.row));
						break;
					case HeuristicFormula.MaxDXDY:
						mH = mHEstimate * (Math.Max (dist, Math.Abs (mNewLocationY - endCell.row)));
						break;
					case HeuristicFormula.DiagonalShortCut:
						float h_diagonal = Math.Min (dist, Math.Abs (mNewLocationY - endCell.row));
						float h_straight = (dist + Math.Abs (mNewLocationY - endCell.row));
						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)));
						break;
					case HeuristicFormula.EuclideanNoSQR:
						mH = mHEstimate * (float)(Math.Pow (dist, 2) + Math.Pow ((mNewLocationY - endCell.row), 2));
						break;
					case HeuristicFormula.Custom1:
						PathFindingPoint dxy = new PathFindingPoint (dist, Math.Abs (endCell.row - mNewLocationY));
						float Orthogonal = Math.Abs (dxy.x - dxy.y);
						float Diagonal = Math.Abs (((dxy.x + dxy.y) - Orthogonal) / 2);
						mH = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
						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;

				PathFinderNodeFast fNodeTmp = mCalcGrid [(endCell.row << mRowCountLog2) + endCell.column];
				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;

				while (fNode.X != fNode.PX || fNode.Y != fNode.PY) {
					mClose.Add (fNode);
					posX = fNode.PX;
					posY = fNode.PY;
					fNodeTmp = mCalcGrid [(posY << mRowCountLog2) + 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;
			}
		}
	}
}