context: using System.Collections.Generic;
  using Pathfinding.Serialization;
  using UnityEngine;
  
  namespace Pathfinding {
  	/** Base class for GridNode and LevelGridNode */
  	public abstract class GridNodeBase : GraphNode {
  		protected GridNodeBase (AstarPath astar) : base(astar) {
  		}
  
  		protected int nodeInGridIndex;
  
  		/** The index of the node in the grid.
  		 * This is x + z*graph.width
  		 * So you can get the X and Z indices using
  		 * \code
  		 * int index = node.NodeInGridIndex;
  		 * int x = index % graph.width;
  		 * int z = index / graph.width;
  		 * // where graph is GridNode.GetGridGraph (node.graphIndex), i.e the graph the nodes are contained in.
  		 * \endcode
  		 */
  		public int NodeInGridIndex { get { return nodeInGridIndex; } set { nodeInGridIndex = value; } }
  	}
  
  	public class GridNode : GridNodeBase {
  		public GridNode (AstarPath astar) : base(astar) {
  		}
  
  		private static GridGraph[] _gridGraphs = new GridGraph[0];
  		public static GridGraph GetGridGraph (uint graphIndex) { return _gridGraphs[(int)graphIndex]; }
  
  #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
  		public GraphNode[] connections;
  		public uint[] connectionCosts;
  #endif
  
  		public static void SetGridGraph (int graphIndex, GridGraph graph) {
  			if (_gridGraphs.Length <= graphIndex) {
  				var gg = new GridGraph[graphIndex+1];
  				for (int i = 0; i < _gridGraphs.Length; i++) gg[i] = _gridGraphs[i];
  				_gridGraphs = gg;
  			}
  
  			_gridGraphs[graphIndex] = graph;
  		}
  
  		protected ushort gridFlags;
  
  		/** Internal use only */
  		internal ushort InternalGridFlags {
  			get { return gridFlags; }
  			set { gridFlags = value; }
  		}
  
  		const int GridFlagsConnectionOffset = 0;
  		const int GridFlagsConnectionBit0 = 1 << GridFlagsConnectionOffset;
  		const int GridFlagsConnectionMask = 0xFF << GridFlagsConnectionOffset;
  
  		const int GridFlagsWalkableErosionOffset = 8;
  		const int GridFlagsWalkableErosionMask = 1 << GridFlagsWalkableErosionOffset;
  
  		const int GridFlagsWalkableTmpOffset = 9;
  		const int GridFlagsWalkableTmpMask = 1 << GridFlagsWalkableTmpOffset;
  
  		const int GridFlagsEdgeNodeOffset = 10;
  		const int GridFlagsEdgeNodeMask = 1 << GridFlagsEdgeNodeOffset;
  
  		/** Returns true if the node has a connection in the specified direction.
  		 * The dir parameter corresponds to directions in the grid as:
  		 * \code
  		 * [0] = -Y
  		 * [1] = +X
  		 * [2] = +Y
  		 * [3] = -X
  		 * [4] = -Y+X
  		 * [5] = +Y+X
  		 * [6] = +Y-X
  		 * [7] = -Y-X
  		 * \endcode
  		 *
  		 * \see SetConnectionInternal
  		 */
  		public bool GetConnectionInternal (int dir) {
  			return (gridFlags >> dir & GridFlagsConnectionBit0) != 0;
  		}
  
  		/** Enables or disables a connection in a specified direction on the graph.
  		 *	\see GetConnectionInternal
  		 */
  		public void SetConnectionInternal (int dir, bool value) {
  			unchecked { gridFlags = (ushort)(gridFlags & ~((ushort)1 << GridFlagsConnectionOffset << dir) | (value ? (ushort)1 : (ushort)0) << GridFlagsConnectionOffset << dir); }
  		}
  
  		/** Sets the state of all grid connections.
  		 * \param connections a bitmask of the connections (bit 0 is the first connection, etc.).
  		 *
  		 * \see SetConnectionInternal
  		 */
  		public void SetAllConnectionInternal (int connections) {
  			unchecked { gridFlags = (ushort)((gridFlags & ~GridFlagsConnectionMask) | (connections << GridFlagsConnectionOffset)); }
  		}
  
  		/** Disables all grid connections from this node.
  		 * \note Other nodes might still be able to get to this node.
  		 * Therefore it is recommended to also disable the relevant connections on adjacent nodes.
  		 */
  		public void ResetConnectionsInternal () {
  			unchecked {
  				gridFlags = (ushort)(gridFlags & ~GridFlagsConnectionMask);
  			}
  		}
  
  		public bool EdgeNode {
  			get {
  				return (gridFlags & GridFlagsEdgeNodeMask) != 0;
  			}
  			set {
  				unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsEdgeNodeMask | (value ? GridFlagsEdgeNodeMask : 0)); }
  			}
  		}
  
  		/** Stores walkability before erosion is applied.
  		 * Used by graph updating.
  		 */
  		public bool WalkableErosion {
  			get {
  				return (gridFlags & GridFlagsWalkableErosionMask) != 0;
  			}
  			set {
  				unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableErosionMask | (value ? (ushort)GridFlagsWalkableErosionMask : (ushort)0)); }
  			}
  		}
  
  		/** Temporary variable used by graph updating */
  		public bool TmpWalkable {
  			get {
  				return (gridFlags & GridFlagsWalkableTmpMask) != 0;
  			}
  			set {
  				unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableTmpMask | (value ? (ushort)GridFlagsWalkableTmpMask : (ushort)0)); }
  			}
  		}
  
  		public override void ClearConnections (bool alsoReverse) {
  			if (alsoReverse) {
  				GridGraph gg = GetGridGraph(GraphIndex);
  				for (int i = 0; i < 8; i++) {
  					GridNode other = gg.GetNodeConnection(this, i);
  					if (other != null) {
  						//Remove reverse connection
  						other.SetConnectionInternal(i < 4 ? ((i + 2) % 4) : (((5-2) % 4) + 4), false);
  					}
  				}
  			}
  
  			ResetConnectionsInternal();
  
  #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
  			if (alsoReverse) {
  				if (connections != null) for (int i = 0; i < connections.Length; i++) connections[i].RemoveConnection(this);
  			}
  			connections = null;
  			connectionCosts = null;
  #endif
  		}
  
  		public override void GetConnections (GraphNodeDelegate del) {
  			GridGraph gg = GetGridGraph(GraphIndex);
  
  			int[] neighbourOffsets = gg.neighbourOffsets;
  			GridNode[] nodes = gg.nodes;
  
  			for (int i = 0; i < 8; i++) {
  				if (GetConnectionInternal(i)) {
  					GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];
  					if (other != null) del(other);
  				}
  			}
  
  #if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
  			if (connections != null) for (int i = 0; i < connections.Length; i++) del(connections[i]);
  #endif
  		}
  
  		public Vector3 ClosestPointOnNode (Vector3 p) {
  			var gg = GetGridGraph(GraphIndex);
  
  			// Convert to graph space
  			p = gg.inverseMatrix.MultiplyPoint3x4(p);
  
  			// Nodes are offset 0.5 graph space nodes
  			float xf = position.x-0.5F;
  			float zf = position.z-0.5f;
  
  			// Calculate graph position of this node
  			int x = nodeInGridIndex % gg.width;
  			int z = nodeInGridIndex / gg.width;
  
  			// Handle the y coordinate separately
  			float y = gg.inverseMatrix.MultiplyPoint3x4((Vector3)p).y;
  
  			var closestInGraphSpace = new Vector3(Mathf.Clamp(xf, x-0.5f, x+0.5f)+0.5f, y, Mathf.Clamp(zf, z-0.5f, z+0.5f)+0.5f);
  
  			// Convert to world space
  			return gg.matrix.MultiplyPoint3x4(closestInGraphSpace);
  		}
  
  		public override bool GetPortal (GraphNode other, List<Vector3> left, List<Vector3> right, bool backwards) {
  			if (backwards) return true;
  
  			GridGraph gg = GetGridGraph(GraphIndex);
  			int[] neighbourOffsets = gg.neighbourOffsets;
  			GridNode[] nodes = gg.nodes;
  
  			for (int i = 0; i < 4; i++) {
  				if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]]) {
  					Vector3 middle = ((Vector3)(position + other.position))*0.5f;
  					Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position-position));
  					cross.Normalize();
  					cross *= gg.nodeSize*0.5f;
  					left.Add(middle - cross);
  					right.Add(middle + cross);
  					return true;
  				}
  			}
  
  			for (int i = 4; i < 8; i++) {
  				if (GetConnectionInternal(i) && other == nodes[nodeInGridIndex + neighbourOffsets[i]]) {
  					bool rClear = false;
  					bool lClear = false;
  					if (GetConnectionInternal(i-4)) {
  						GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[i-4]];
  						if (n2.Walkable && n2.GetConnectionInternal((i-4+1)%4)) {
  							rClear = true;
  						}
  					}
  
  					if (GetConnectionInternal((i-4+1)%4)) {
  						GridNode n2 = nodes[nodeInGridIndex + neighbourOffsets[(i-4+1)%4]];
  						if (n2.Walkable && n2.GetConnectionInternal(i-4)) {
  							lClear = true;
  						}
  					}
  
  					Vector3 middle = ((Vector3)(position + other.position))*0.5f;
  					Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position-position));
  					cross.Normalize();
  					cross *= gg.nodeSize*1.4142f;
  					left.Add(middle - (lClear ? cross : Vector3.zero));
  					right.Add(middle + (rClear ? cross : Vector3.zero));
  					return true;
  				}
  			}
  
  			return false;
  		}
  
  		public override void FloodFill (Stack<GraphNode> stack, uint region) {
  			GridGraph gg = GetGridGraph(GraphIndex);
  
  			int[] neighbourOffsets = gg.neighbourOffsets;
  			GridNode[] nodes = gg.nodes;
  
  			for (int i = 0; i < 8; i++) {
  				if (GetConnectionInternal(i)) {
  
gt: 					GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];