// Copyright (c) 2013-2015 Robert Rouhani <robert.rouhani@gmail.com> and other contributors (see CONTRIBUTORS file).
// Licensed under the MIT License - https://raw.github.com/Robmaister/SharpNav/master/LICENSE

using System;
using System.Collections.Generic;

using SharpNav.Collections.Generic;
using SharpNav.Geometry;
using SharpNav.Pathfinding;

#if MONOGAME
using Vector3 = Microsoft.Xna.Framework.Vector3;
#elif OPENTK
using Vector3 = OpenTK.Vector3;
#elif SHARPDX
using Vector3 = SharpDX.Vector3;
#endif

namespace SharpNav
{
	/// <summary>
	/// Do pathfinding calculations on the TiledNavMesh
	/// </summary>
	public class NavMeshQuery
	{
		private const float H_SCALE = 0.999f;

		private TiledNavMesh nav;
		private float[] areaCost; 
		private NodePool tinyNodePool;
		private NodePool nodePool;
		private PriorityQueue<Node> openList;
		private QueryData query;
		private Random rand;

		/// <summary>
		/// Initializes a new instance of the <see cref="NavMeshQuery"/> class.
		/// </summary>
		/// <param name="nav">The navigation mesh to query.</param>
		/// <param name="maxNodes">The maximum number of nodes that can be queued in a query.</param>
		public NavMeshQuery(TiledNavMesh nav, int maxNodes)
			: this(nav, maxNodes, new Random())
		{
		}

		/// <summary>
		/// Initializes a new instance of the <see cref="NavMeshQuery"/> class.
		/// </summary>
		/// <param name="nav">The navigation mesh to query.</param>
		/// <param name="maxNodes">The maximum number of nodes that can be queued in a query.</param>
		/// <param name="rand">A random number generator for use in methods like <see cref="NavMeshQuery.FindRandomPoint()"/></param>
		public NavMeshQuery(TiledNavMesh nav, int maxNodes, Random rand)
		{
			this.nav = nav;

			areaCost = new float[byte.MaxValue + 1];
			for (int i = 0; i < areaCost.Length; i++)
				areaCost[i] = 1.0f;

			nodePool = new NodePool(maxNodes, MathHelper.NextPowerOfTwo(maxNodes / 4));
			tinyNodePool = new NodePool(64, 32);
			openList = new PriorityQueue<Node>(maxNodes);

			this.rand = rand;
		}

		/// <summary>
		/// The cost between two points may vary depending on the type of polygon.
		/// </summary>
		/// <param name="pa">Point A</param>
		/// <param name="pb">Point B</param>
		/// <param name="curPoly">Current polygon</param>
		/// <returns>Cost</returns>
		public float GetCost(Vector3 pa, Vector3 pb, Poly curPoly)
		{
			return (pa - pb).Length() * areaCost[(int)curPoly.Area.Id];
		}

		public TiledNavMesh NavMesh
		{
			get
			{
				return nav;
			}
		}

		/// <summary>
		/// Finds a random point on a polygon.
		/// </summary>
		/// <param name="tile">The current mesh tile</param>
		/// <param name="poly">The current polygon</param>
		/// <param name="polyRef">Polygon reference</param>
		/// <returns>Resulting random point</returns>
		public Vector3 FindRandomPointOnPoly(MeshTile tile, Poly poly, int polyRef)
		{
			Vector3 result;
			this.FindRandomPointOnPoly(tile, poly, polyRef, out result);
			return result;
		}

		/// <summary>
		/// Finds a random point on a polygon.
		/// </summary>
		/// <param name="tile">The current mesh tile</param>
		/// <param name="poly">The current polygon</param>
		/// <param name="polyRef">Polygon reference</param>
		/// <param name="randomPt">Resulting random point</param>
		public void FindRandomPointOnPoly(MeshTile tile, Poly poly, int polyRef, out Vector3 randomPt)
		{
			Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
			float[] areas = new float[PathfindingCommon.VERTS_PER_POLYGON];
			for (int j = 0; j < poly.VertCount; j++)
				verts[j] = tile.Verts[poly.Verts[j]];

			float s = (float)rand.NextDouble();
			float t = (float)rand.NextDouble();

			PathfindingCommon.RandomPointInConvexPoly(verts, poly.VertCount, areas, s, t, out randomPt);

			//TODO bad state again.
			float h = 0.0f;
			if (!GetPolyHeight(polyRef, randomPt, ref h))
				throw new InvalidOperationException("Outside bounds?");

			randomPt.Y = h;
		}

		/// <summary>
		/// Finds a random point somewhere in the navigation mesh.
		/// </summary>
		/// <returns>Resulting random point.</returns>
		public NavPoint FindRandomPoint()
		{
			NavPoint result;
			this.FindRandomPoint(out result);
			return result;
		}

		/// <summary>
		/// Finds a random point somewhere in the navigation mesh.
		/// </summary>
		/// <param name="randomPoint">Resulting random point.</param>
		public void FindRandomPoint(out NavPoint randomPoint)
		{
			//TODO we're object-oriented, can prevent this state from ever happening.
			if (nav == null)
				throw new InvalidOperationException("TODO prevent this state from ever occuring");

			//randomly pick one tile
			//assume all tiles cover roughly the same area
			MeshTile tile = null;
			float tsum = 0.0f;
			
			for (int i = 0; i < nav.TileCount; i++)
			{
				MeshTile t = nav[i];
				
				if (t == null || t.Header == null)
					continue;

				//choose random tile using reservoir sampling
				float area = 1.0f;
				tsum += area;
				float u = (float)rand.NextDouble();
				if (u * tsum <= area)
					tile = t;
			}

			//TODO why?
			if (tile == null)
				throw new InvalidOperationException("No tiles?");

			//randomly pick one polygon weighted by polygon area
			Poly poly = null;
			int polyRef = 0;
			int polyBase = nav.GetPolyRefBase(tile);

			float areaSum = 0.0f;
			for (int i = 0; i < tile.Header.PolyCount; i++)
			{
				Poly p = tile.Polys[i];

				//don't return off-mesh connection polygons
				if (p.PolyType != PolygonType.Ground)
					continue;

				int reference = polyBase | i;

				//calculate area of polygon
				float polyArea = 0.0f;
				float area;
				for (int j = 2; j < p.VertCount; j++)
				{
					Triangle3.Area2D(ref tile.Verts[p.Verts[0]], ref tile.Verts[p.Verts[j - 1]], ref tile.Verts[p.Verts[j]], out area);
					polyArea += area;
				}

				//choose random polygon weighted by area, usig resevoir sampling
				areaSum += polyArea;
				float u = (float)rand.NextDouble();
				if (u * areaSum <= polyArea)
				{
					poly = p;
					polyRef = reference;
				}
			}

			//TODO why?
			if (poly == null)
				throw new InvalidOperationException("No polys?");

			//randomRef = polyRef;
			Vector3 randomPt;
			FindRandomPointOnPoly(tile, poly, polyRef, out randomPt);

			randomPoint = new NavPoint(polyRef, randomPt);
		}

		/// <summary>
		/// Finds a random point in a NavMesh within a specified circle.
		/// </summary>
		/// <param name="center">The center point.</param>
		/// <param name="radius">The maximum distance away from the center that the random point can be.</param>
		/// <returns>A random point within the specified circle.</returns>
		public NavPoint FindRandomPointAroundCircle(NavPoint center, float radius)
		{
			NavPoint result;
			this.FindRandomPointAroundCircle(center, radius, out result);
			return result;
		}

		/// <summary>
		/// Finds a random point in a NavMesh within a specified circle.
		/// </summary>
		/// <param name="center">The center point.</param>
		/// <param name="radius">The maximum distance away from the center that the random point can be.</param>
		/// <param name="randomPoint">A random point within the specified circle.</param>
		public void FindRandomPointAroundCircle(NavPoint center, float radius, out NavPoint randomPoint)
		{
			//TODO fix state
			if (nav == null || nodePool == null || openList == null)
				throw new InvalidOperationException("Something null");

			//validate input
			if (center.Polygon == 0)
				throw new ArgumentOutOfRangeException("startRef", "Null poly reference");

			if (!nav.IsValidPolyRef(center.Polygon))
				throw new ArgumentException("startRef", "Poly reference is not valid for this navmesh");

			MeshTile startTile;
			Poly startPoly;
			nav.TryGetTileAndPolyByRefUnsafe(center.Polygon, out startTile, out startPoly);

			nodePool.Clear();
			openList.Clear();

			Node startNode = nodePool.GetNode(center.Polygon);
			startNode.Pos = center.Position;
			startNode.ParentIdx = 0;
			startNode.cost = 0;
			startNode.total = 0;
			startNode.Id = center.Polygon;
			startNode.Flags = NodeFlags.Open;
			openList.Push(startNode);

			float radiusSqr = radius * radius;
			float areaSum = 0.0f;

			MeshTile randomTile = null;
			Poly randomPoly = null;
			int randomPolyRef = 0;

			while (openList.Count > 0)
			{
				Node bestNode = openList.Pop();
				SetNodeFlagClosed(ref bestNode);

				//get poly and tile
				int bestRef = bestNode.Id;
				MeshTile bestTile;
				Poly bestPoly;
				nav.TryGetTileAndPolyByRefUnsafe(bestRef, out bestTile, out bestPoly);

				//place random locations on ground
				if (bestPoly.PolyType == PolygonType.Ground)
				{
					//calculate area of polygon
					float polyArea = 0.0f;
					float area;
					for (int j = 2; j < bestPoly.VertCount; j++)
					{
						Triangle3.Area2D(ref bestTile.Verts[bestPoly.Verts[0]], ref bestTile.Verts[bestPoly.Verts[j - 1]], ref bestTile.Verts[bestPoly.Verts[j]], out area);
						polyArea += area;
					}

					//choose random polygon weighted by area using resevoir sampling
					areaSum += polyArea;
					float u = (float)rand.NextDouble();
					if (u * areaSum <= polyArea)
					{
						randomTile = bestTile;
						randomPoly = bestPoly;
						randomPolyRef = bestRef;
					}
				}

				//get parent poly and tile
				int parentRef = 0;
				MeshTile parentTile;
				Poly parentPoly;
				if (bestNode.ParentIdx != 0)
					parentRef = nodePool.GetNodeAtIdx(bestNode.ParentIdx).Id;
				if (parentRef != 0)
					nav.TryGetTileAndPolyByRefUnsafe(parentRef, out parentTile, out parentPoly);

				for (int i = bestPoly.FirstLink; i != Link.Null; i = bestTile.Links[i].Next)
				{
					Link link = bestTile.Links[i];
					int neighbourRef = link.Reference;

					//skip invalid neighbours and do not follor back to parent
					if (neighbourRef == 0 || neighbourRef == parentRef)
						continue;

					//expand to neighbour
					MeshTile neighbourTile;
					Poly neighbourPoly;
					nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);

					//find edge and calculate distance to edge
					Vector3 va = new Vector3();
					Vector3 vb = new Vector3();
					if (!GetPortalPoints(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, ref va, ref vb))
						continue;

					//if circle isn't touching next polygon, skip it
					float tseg;
					float distSqr = Distance.PointToSegment2DSquared(ref center.Position, ref va, ref vb, out tseg);
					if (distSqr > radiusSqr)
						continue;

					Node neighbourNode = nodePool.GetNode(neighbourRef);
					if (neighbourNode == null)
						continue;

					if (IsInClosedList(neighbourNode))
						continue;

					//cost
					if (neighbourNode.Flags == 0)
						neighbourNode.Pos = Vector3.Lerp(va, vb, 0.5f);

					float total = bestNode.total + (bestNode.Pos - neighbourNode.Pos).Length();

					//node is already in open list and new result is worse, so skip
					if (IsInOpenList(neighbourNode) && total >= neighbourNode.total)
						continue;

					neighbourNode.Id = neighbourRef;
					neighbourNode.Flags = RemoveNodeFlagClosed(neighbourNode);
					neighbourNode.ParentIdx = nodePool.GetNodeIdx(bestNode);
					neighbourNode.total = total;

					if (IsInOpenList(neighbourNode))
					{
						openList.Modify(neighbourNode);
					}
					else
					{
						neighbourNode.Flags = NodeFlags.Open;
						openList.Push(neighbourNode);
					}
				}
			}

			//TODO invalid state.
			if (randomPoly == null)
				throw new InvalidOperationException("Poly null?");

			Vector3 randomPt;
			FindRandomPointOnPoly(randomTile, randomPoly, randomPolyRef, out randomPt);

			randomPoint = new NavPoint(randomPolyRef, randomPt);
		}

		/// <summary>
		/// Find a path from the start polygon to the end polygon.
		/// -If the end polygon can't be reached, the last polygon will be nearest the end polygon
		/// -If the path array is too small, it will be filled as far as possible 
		/// -start and end positions are used to calculate traversal costs
		/// </summary>
		/// <param name="startPt">The start point.</param>
		/// <param name="endPt">The end point.</param>
		/// <param name="path">The path of polygon references</param>
		/// <returns>True, if path found. False, if otherwise.</returns>
		public bool FindPath(ref NavPoint startPt, ref NavPoint endPt, List<int> path)
		{
			//reset path of polygons
			path.Clear();

			int startRef = startPt.Polygon;
			Vector3 startPos = startPt.Position;
			int endRef = endPt.Polygon;
			Vector3 endPos = endPt.Position;

			if (startRef == 0 || endRef == 0)
				return false;

			//path can't store any elements
			if (path.Capacity == 0)
				return false;

			//validate input
			if (!nav.IsValidPolyRef(startRef) || !nav.IsValidPolyRef(endRef))
				return false;

			//special case: both start and end are in the same polygon
			if (startRef == endRef)
			{
				path.Add(startRef);
				return true;
			}

			nodePool.Clear();
			openList.Clear();

			//initial node is located at the starting position
			Node startNode = nodePool.GetNode(startRef);
			startNode.Pos = startPos;
			startNode.ParentIdx = 0;
			startNode.cost = 0;
			startNode.total = (startPos - endPos).Length() * H_SCALE;
			startNode.Id = startRef;
			startNode.Flags = NodeFlags.Open;
			openList.Push(startNode);

			Node lastBestNode = startNode;
			float lastBestTotalCost = startNode.total;

			while (openList.Count > 0)
			{
				//remove node from open list and put it in closed list
				Node bestNode = openList.Pop();
				SetNodeFlagClosed(ref bestNode);

				//reached the goal. stop searching
				if (bestNode.Id == endRef)
				{
					lastBestNode = bestNode;
					break;
				}

				//get current poly and tile
				int bestRef = bestNode.Id;
				MeshTile bestTile;
				Poly bestPoly;
				nav.TryGetTileAndPolyByRefUnsafe(bestRef, out bestTile, out bestPoly);

				//get parent poly and tile
				int parentRef = 0;
				MeshTile parentTile;
				Poly parentPoly;
				if (bestNode.ParentIdx != 0)
					parentRef = nodePool.GetNodeAtIdx(bestNode.ParentIdx).Id;
				if (parentRef != 0)
					nav.TryGetTileAndPolyByRefUnsafe(parentRef, out parentTile, out parentPoly);

				//examine neighbors
				for (int i = bestPoly.FirstLink; i != Link.Null; i = bestTile.Links[i].Next)
				{
					int neighbourRef = bestTile.Links[i].Reference;

					//skip invalid ids and do not expand back to where we came from
					if (neighbourRef == 0 || neighbourRef == parentRef)
						continue;

					//get neighbour poly and tile
					MeshTile neighbourTile;
					Poly neighbourPoly;
					nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);

					Node neighbourNode = nodePool.GetNode(neighbourRef);
					if (neighbourNode == null)
						continue;

					//if node is visited the first time, calculate node position
					if (neighbourNode.Flags == 0)
					{
						GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, ref neighbourNode.Pos);
					}

					//calculate cost and heuristic
					float cost = 0;
					float heuristic = 0;

					//special case for last node
					if (neighbourRef == endRef)
					{
						//cost
						float curCost = GetCost(bestNode.Pos, neighbourNode.Pos, bestPoly);
						float endCost = GetCost(neighbourNode.Pos, endPos, neighbourPoly);

						cost = bestNode.cost + curCost + endCost;
						heuristic = 0;
					}
					else
					{
						//cost
						float curCost = GetCost(bestNode.Pos, neighbourNode.Pos, bestPoly);
						
						cost = bestNode.cost + curCost;
						heuristic = (neighbourNode.Pos - endPos).Length() * H_SCALE; 
					}

					float total = cost + heuristic;

					//the node is already in open list and new result is worse, skip
					if (IsInOpenList(neighbourNode) && total >= neighbourNode.total)
						continue;

					//the node is already visited and processesd, and the new result is worse, skip
					if (IsInClosedList(neighbourNode) && total >= neighbourNode.total)
						continue;

					//add or update the node
					neighbourNode.ParentIdx = nodePool.GetNodeIdx(bestNode);
					neighbourNode.Id = neighbourRef;
					neighbourNode.Flags = RemoveNodeFlagClosed(neighbourNode);
					neighbourNode.cost = cost;
					neighbourNode.total = total;

					if (IsInOpenList(neighbourNode))
					{
						//already in open, update node location
						openList.Modify(neighbourNode);
					}
					else
					{
						//put the node in the open list
						SetNodeFlagOpen(ref neighbourNode);
						openList.Push(neighbourNode);
					}

					//update nearest node to target so far
					if (heuristic < lastBestTotalCost)
					{
						lastBestTotalCost = heuristic;
						lastBestNode = neighbourNode;
					}
				}
			}

			//save path
			Node node = lastBestNode;
			do
			{
				path.Add(node.Id);
				if (path.Count >= path.Capacity)
					break;
		
				node = nodePool.GetNodeAtIdx(node.ParentIdx);
			}
			while (node != null);
			
			//reverse the path since it's backwards
			path.Reverse();

			return true;
		}

		/// <summary>
		/// Add vertices and portals to a regular path computed from the method FindPath().
		/// </summary>
		/// <param name="startPos">Starting position</param>
		/// <param name="endPos">Ending position</param>
		/// <param name="path">Path of polygon references</param>
		/// <param name="pathSize">Length of path</param>
		/// <param name="straightPath">An array of points on the straight path</param>
		/// <param name="straightPathFlags">An array of flags</param>
		/// <param name="straightPathRefs">An array of polygon references</param>
		/// <param name="straightPathCount">The number of points on the path</param>
		/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
		/// <param name="options">Options flag</param>
		/// <returns>True, if path found. False, if otherwise.</returns>
		public bool FindStraightPath(Vector3 startPos, Vector3 endPos, int[] path, int pathSize, Vector3[] straightPath, int[] straightPathFlags, int[] straightPathRefs, ref int straightPathCount, int maxStraightPath, PathBuildFlags options)
		{
			straightPathCount = 0;

			if (path.Length == 0)
				return false;

			bool stat = false;

			Vector3 closestStartPos = new Vector3();
			ClosestPointOnPolyBoundary(path[0], startPos, ref closestStartPos);

			Vector3 closestEndPos = new Vector3();
			ClosestPointOnPolyBoundary(path[pathSize - 1], endPos, ref closestEndPos);

			stat = AppendVertex(closestStartPos, PathfindingCommon.STRAIGHTPATH_START, path[0], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

			if (!stat)
				return true;

			if (pathSize > 1)
			{
				Vector3 portalApex = closestStartPos;
				Vector3 portalLeft = portalApex;
				Vector3 portalRight = portalApex;
				int apexIndex = 0;
				int leftIndex = 0;
				int rightIndex = 0;

				PolygonType leftPolyType = 0;
				PolygonType rightPolyType = 0;

				int leftPolyRef = path[0];
				int rightPolyRef = path[0];

				for (int i = 0; i < pathSize; i++)
				{
					Vector3 left = new Vector3();
					Vector3 right = new Vector3();
					PolygonType fromType = 0, toType = 0;

					if (i + 1 < pathSize)
					{
						//next portal
						if (GetPortalPoints(path[i], path[i + 1], ref left, ref right, ref fromType, ref toType) == false)
						{
							//failed to get portal points means path[i + 1] is an invalid polygon
							//clamp end point to path[i] and return path so far
							if (ClosestPointOnPolyBoundary(path[i], endPos, ref closestEndPos) == false)
							{
								//first polygon is invalid
								return false;
							}

							if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
							{
								//append portals
								stat = AppendPortals(apexIndex, i, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);
							}

							stat = AppendVertex(closestEndPos, 0, path[i], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

							return true;
						}

						//if starting really close to the portal, advance
						if (i == 0)
						{
							float t;
							if (Distance.PointToSegment2DSquared(ref portalApex, ref left, ref right, out t) < 0.001 * 0.001)
								continue;
						}
					}
					else
					{
						//end of the path
						left = closestEndPos;
						right = closestEndPos;

						fromType = toType = PolygonType.Ground;
					}

					//right vertex
					float triArea2D;
					Triangle3.Area2D(ref portalApex, ref portalRight, ref right, out triArea2D);
					if (triArea2D <= 0.0)
					{
						Triangle3.Area2D(ref portalApex, ref portalLeft, ref right, out triArea2D);
						if (portalApex == portalRight || triArea2D > 0.0)
						{
							portalRight = right;
							rightPolyRef = (i + 1 < pathSize) ? path[i + 1] : 0;
							rightPolyType = toType;
							rightIndex = i;
						}
						else
						{
							//append portals along current straight path segment
							if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
							{
								stat = AppendPortals(apexIndex, leftIndex, portalLeft, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);

								if (stat != true)
									return true;
							}

							portalApex = portalLeft;
							apexIndex = leftIndex;

							int flags = 0;
							if (leftPolyRef == 0)
								flags = PathfindingCommon.STRAIGHTPATH_END;
							else if (leftPolyType == PolygonType.OffMeshConnection)
								flags = PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION;

							int reference = leftPolyRef;

							//append or update vertex
							stat = AppendVertex(portalApex, flags, reference, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

							if (stat != true)
								return true;

							portalLeft = portalApex;
							portalRight = portalApex;
							leftIndex = apexIndex;
							rightIndex = apexIndex;

							//restart
							i = apexIndex;

							continue;
						}
					}

					//left vertex
					Triangle3.Area2D(ref portalApex, ref portalLeft, ref left, out triArea2D);
					if (triArea2D >= 0.0)
					{
						Triangle3.Area2D(ref portalApex, ref portalRight, ref left, out triArea2D);
						if (portalApex == portalLeft || triArea2D < 0.0f)
						{
							portalLeft = left;
							leftPolyRef = (i + 1 < pathSize) ? path[i + 1] : 0;
							leftPolyType = toType;
							leftIndex = i;
						}
						else
						{
							if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
							{
								stat = AppendPortals(apexIndex, rightIndex, portalRight, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);

								if (stat != true)
									return true;
							}

							portalApex = portalRight;
							apexIndex = rightIndex;

							int flags = 0;
							if (rightPolyRef == 0)
								flags = PathfindingCommon.STRAIGHTPATH_END;
							else if (rightPolyType == PolygonType.OffMeshConnection)
								flags = PathfindingCommon.STRAIGHTPATH_OFFMESH_CONNECTION;

							int reference = rightPolyRef;

							//append or update vertex
							stat = AppendVertex(portalApex, flags, reference, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

							if (stat != true)
								return true;

							portalLeft = portalApex;
							portalRight = portalApex;
							leftIndex = apexIndex;
							rightIndex = apexIndex;

							//restart 
							i = apexIndex;

							continue;
						}
					}
				}

				//append portals along the current straight line segment
				if ((options & (PathBuildFlags.AreaCrossingVertices | PathBuildFlags.AllCrossingVertices)) != 0)
				{
					stat = AppendPortals(apexIndex, pathSize - 1, closestEndPos, path, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath, options);

					if (stat != true)
						return true;
				}
			}

			stat = AppendVertex(closestEndPos, PathfindingCommon.STRAIGHTPATH_END, 0, straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

			return true;
		}

		/// <summary>
		/// This method is optimized for small delta movement and a small number of polygons.
		/// If movement distance is too large, the result will form an incomplete path.
		/// </summary>
		/// <param name="startPoint">The start point.</param>
		/// <param name="endPos">End position</param>
		/// <param name="resultPos">Intermediate point</param>
		/// <param name="visited">Visited polygon references</param>
		/// <returns>True, if point found. False, if otherwise.</returns>
		public bool MoveAlongSurface(NavPoint startPoint, Vector3 endPos, ref Vector3 resultPos, List<int> visited)
		{
			if (nav == null)
				return false;
			if (tinyNodePool == null)
				return false;

			visited.Clear();

			//validate input
			if (startPoint.Polygon == 0)
				return false;
			if (!nav.IsValidPolyRef(startPoint.Polygon))
				return false;

			int MAX_STACK = 48;
			Queue<Node> nodeQueue = new Queue<Node>(MAX_STACK);

			tinyNodePool.Clear();

			Node startNode = tinyNodePool.GetNode(startPoint.Polygon);
			startNode.ParentIdx = 0;
			startNode.cost = 0;
			startNode.total = 0;
			startNode.Id = startPoint.Polygon;
			startNode.Flags = NodeFlags.Closed;
			nodeQueue.Enqueue(startNode);

			Vector3 bestPos = startPoint.Position;
			float bestDist = float.MaxValue;
			Node bestNode = null;

			//search constraints
			Vector3 searchPos = Vector3.Lerp(startPoint.Position, endPos, 0.5f);
			float searchRad = (startPoint.Position - endPos).Length() / 2.0f + 0.001f;
			float searchRadSqr = searchRad * searchRad;

			Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
			
			while (nodeQueue.Count > 0)
			{
				//pop front
				Node curNode = nodeQueue.Dequeue();

				//get poly and tile
				int curRef = curNode.Id;
				MeshTile curTile;
				Poly curPoly;
				nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);

				//collect vertices
				int nverts = curPoly.VertCount;
				for (int i = 0; i < nverts; i++)
					verts[i] = curTile.Verts[curPoly.Verts[i]];

				//if target is inside poly, stop search
				if (Containment.PointInPoly(endPos, verts, nverts))
				{
					bestNode = curNode;
					bestPos = endPos;
					break;
				}

				//find wall edges and find nearest point inside walls
				for (int i = 0, j = curPoly.VertCount - 1; i < curPoly.VertCount; j = i++)
				{
					//find links to neighbors
					List<int> neis = new List<int>(8);

					if ((curPoly.Neis[j] & Link.External) != 0)
					{
						//tile border
						for (int k = curPoly.FirstLink; k != Link.Null; k = curTile.Links[k].Next)
						{
							Link link = curTile.Links[k];
							if (link.Edge == j)
							{
								if (link.Reference != 0)
								{
									MeshTile neiTile;
									Poly neiPoly;
									nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out neiTile, out neiPoly);
									
									if (neis.Count < neis.Capacity)
										neis.Add(link.Reference);
								}
							}
						}
					}
					else if (curPoly.Neis[j] != 0)
					{
						int idx = curPoly.Neis[j] - 1;
						int reference = nav.GetPolyRefBase(curTile) | idx;
						neis.Add(reference); //internal edge, encode id
					}

					if (neis.Count == 0)
					{
						//wall edge, calculate distance
						float tseg = 0;
						float distSqr = Distance.PointToSegment2DSquared(ref endPos, ref verts[j], ref verts[i], out tseg);
						if (distSqr < bestDist)
						{
							//update nearest distance
							bestPos = Vector3.Lerp(verts[j], verts[i], tseg);
							bestDist = distSqr;
							bestNode = curNode;
						}
					}
					else
					{
						for (int k = 0; k < neis.Count; k++)
						{
							//skip if no node can be allocated
							Node neighbourNode = tinyNodePool.GetNode(neis[k]);
							if (neighbourNode == null)
								continue;
							
							//skip if already visited
							if ((neighbourNode.Flags & NodeFlags.Closed) != 0)
								continue;

							//skip the link if too far from search constraint
							float distSqr = Distance.PointToSegment2DSquared(ref searchPos, ref verts[j], ref verts[i]);
							if (distSqr > searchRadSqr)
								continue;

							//mark the node as visited and push to queue
							if (nodeQueue.Count < MAX_STACK)
							{
								neighbourNode.ParentIdx = tinyNodePool.GetNodeIdx(curNode);
								neighbourNode.Flags |= NodeFlags.Closed;
								nodeQueue.Enqueue(neighbourNode);
							}
						}
					}
				}
			}

			if ((endPos - bestPos).Length() > 1f)
				return false;

			if (bestNode != null)
			{
				//save the path
				Node node = bestNode;
				do
				{
					visited.Add(node.Id);
					if (visited.Count >= visited.Capacity)
						break;

					node = tinyNodePool.GetNodeAtIdx(node.ParentIdx);
				}
				while (node != null);

				//reverse the path since it's backwards
				visited.Reverse();
			}

			resultPos = bestPos;
			
			return true;
		}

		/// <summary>
		/// Initialize a sliced path, which is used mostly for crowd pathfinding.
		/// </summary>
		/// <param name="startPoint">The start point.</param>
		/// <param name="endPoint">The end point.</param>
		/// <returns>True if path initialized, false otherwise</returns>
		public bool InitSlicedFindPath(NavPoint startPoint, NavPoint endPoint)
		{
			//init path state
			query = new QueryData();
			query.Status = false;
			query.StartRef = startPoint.Polygon;
			query.EndRef = endPoint.Polygon;
			query.StartPos = startPoint.Position;
			query.EndPos = endPoint.Position;

			if (query.StartRef == 0 || query.EndRef == 0)
				return false;

			//validate input
			if (!nav.IsValidPolyRef(startPoint.Polygon) || !nav.IsValidPolyRef(endPoint.Polygon))
				return false;

			if (startPoint.Polygon == endPoint.Polygon)
			{
				query.Status = true;
				return true;
			}

			nodePool.Clear();
			openList.Clear();

			Node startNode = nodePool.GetNode(startPoint.Polygon);
			startNode.Pos = startPoint.Position;
			startNode.ParentIdx = 0;
			startNode.cost = 0;
			startNode.total = (endPoint.Position - startPoint.Position).Length() * H_SCALE;
			startNode.Id = startPoint.Polygon;
			startNode.Flags = NodeFlags.Open;
			openList.Push(startNode);

			query.Status = true;
			query.LastBestNode = startNode;
			query.LastBestNodeCost = startNode.total;

			return query.Status;
		}

		/// <summary>
		/// Update the sliced path as agents move across the path.
		/// </summary>
		/// <param name="maxIter">Maximum iterations</param>
		/// <param name="doneIters">Number of times iterated through</param>
		/// <returns>True if updated, false if not</returns>
		public bool UpdateSlicedFindPath(int maxIter, ref int doneIters)
		{
			if (query.Status != true)
				return query.Status;

			//make sure the request is still valid
			if (!nav.IsValidPolyRef(query.StartRef) || !nav.IsValidPolyRef(query.EndRef))
			{
				query.Status = false;
				return false;
			}

			int iter = 0;
			while (iter < maxIter && !openList.Empty())
			{
				iter++;

				//remove node from open list and put it in closed list
				Node bestNode = openList.Pop();
				SetNodeFlagClosed(ref bestNode);

				//reached the goal, stop searching
				if (bestNode.Id == query.EndRef)
				{
					query.LastBestNode = bestNode;
					query.Status = true;
					doneIters = iter;
					return query.Status;
				}

				//get current poly and tile
				int bestRef = bestNode.Id;
				MeshTile bestTile;
				Poly bestPoly;
				if (nav.TryGetTileAndPolyByRef(bestRef, out bestTile, out bestPoly) == false)
				{
					//the polygon has disappeared during the sliced query, fail
					query.Status = false;
					doneIters = iter;
					return query.Status;
				}

				//get parent poly and tile
				int parentRef = 0;
				MeshTile parentTile;
				Poly parentPoly;
				if (bestNode.ParentIdx != 0)
					parentRef = nodePool.GetNodeAtIdx(bestNode.ParentIdx).Id;
				if (parentRef != 0)
				{
					if (nav.TryGetTileAndPolyByRef(parentRef, out parentTile, out parentPoly) == false)
					{
						//the polygon has disappeared during the sliced query, fail
						query.Status = false;
						doneIters = iter;
						return query.Status;
					}
				}

				for (int i = bestPoly.FirstLink; i != Link.Null; i = bestTile.Links[i].Next)
				{
					int neighbourRef = bestTile.Links[i].Reference;

					//skip invalid ids and do not expand back to where we came from
					if (neighbourRef == 0 || neighbourRef == parentRef)
						continue;

					//get neighbour poly and tile
					MeshTile neighbourTile;
					Poly neighbourPoly;
					nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);

					Node neighbourNode = nodePool.GetNode(neighbourRef);
					if (neighbourNode == null)
						continue;

					if (neighbourNode.Flags == 0)
					{
						GetEdgeMidPoint(bestRef, bestPoly, bestTile, neighbourRef, neighbourPoly, neighbourTile, ref neighbourNode.Pos);
					}

					//calculate cost and heuristic
					float cost = 0;
					float heuristic = 0;

					//special case for last node
					if (neighbourRef == query.EndRef)
					{
						//cost
						float curCost = GetCost(bestNode.Pos, neighbourNode.Pos, bestPoly);
						float endCost = GetCost(neighbourNode.Pos, query.EndPos, neighbourPoly);

						cost = bestNode.cost + curCost + endCost;
						heuristic = 0;
					}
					else
					{
						//cost
						float curCost = GetCost(bestNode.Pos, neighbourNode.Pos, bestPoly);

						cost = bestNode.cost + curCost;
						heuristic = (neighbourNode.Pos - query.EndPos).Length() * H_SCALE;
					}

					float total = cost + heuristic;

					//the node is already in open list and new result is worse, skip
					if (IsInOpenList(neighbourNode) && total >= neighbourNode.total)
						continue;

					//the node is already visited and processesd, and the new result is worse, skip
					if (IsInClosedList(neighbourNode) && total >= neighbourNode.total)
						continue;

					//add or update the node
					neighbourNode.ParentIdx = nodePool.GetNodeIdx(bestNode);
					neighbourNode.Id = neighbourRef;
					neighbourNode.Flags = RemoveNodeFlagClosed(neighbourNode);
					neighbourNode.cost = cost;
					neighbourNode.total = total;

					if (IsInOpenList(neighbourNode))
					{
						//already in open, update node location
						openList.Modify(neighbourNode);
					}
					else
					{
						//put the node in the open list
						SetNodeFlagOpen(ref neighbourNode);
						openList.Push(neighbourNode);
					}

					//update nearest node to target so far
					if (heuristic < query.LastBestNodeCost)
					{
						query.LastBestNodeCost = heuristic;
						query.LastBestNode = neighbourNode;
					}
				}
			}

			//exhausted all nodes, but could not find path
			if (openList.Empty())
			{
				query.Status = true;
			}

			doneIters = iter;

			return query.Status;
		}

		/// <summary>
		/// Save the sliced path 
		/// </summary>
		/// <param name="path">The path in terms of polygon references</param>
		/// <param name="pathCount">The path length</param>
		/// <param name="maxPath">The maximum path length allowed</param>
		/// <returns>True if the path is saved, false if not</returns>
		public bool FinalizeSlicedFindPath(int[] path, ref int pathCount, int maxPath)
		{
			pathCount = 0;

			if (query.Status == false)
			{
				query = new QueryData();
				return false;
			}

			int n = 0;

			if (query.StartRef == query.EndRef)
			{
				//special case: the search starts and ends at the same poly
				path[n++] = query.StartRef;
			}
			else
			{
				//reverse the path
				Node prev = null;
				Node node = query.LastBestNode;
				do
				{
					Node next = nodePool.GetNodeAtIdx(node.ParentIdx);
					node.ParentIdx = nodePool.GetNodeIdx(prev);
					prev = node;
					node = next;
				}
				while (node != null);

				//store path
				node = prev;
				do
				{
					path[n++] = node.Id;
					if (n >= maxPath)
						break;

					node = nodePool.GetNodeAtIdx(node.ParentIdx);
				}
				while (node != null);
			}

			//reset query
			query = new QueryData();

			//remember to update the path length
			pathCount = n;

			return true;
		}

		/// <summary>
		/// Save a partial path
		/// </summary>
		/// <param name="existing">Existing path</param>
		/// <param name="existingSize">Existing path's length</param>
		/// <param name="path">New path</param>
		/// <param name="pathCount">New path's length</param>
		/// <param name="maxPath">Maximum path length allowed</param>
		/// <returns>True if path saved, false if not</returns>
		public bool FinalizedSlicedPathPartial(int[] existing, int existingSize, int[] path, ref int pathCount, int maxPath)
		{
			pathCount = 0;

			if (existingSize == 0)
			{
				return false;
			}

			if (query.Status == false)
			{
				query = new QueryData();
				return false;
			}

			int n = 0;

			if (query.StartRef == query.EndRef)
			{
				//special case: the search starts and ends at the same poly
				path[n++] = query.StartRef;
			}
			else
			{
				//find furthest existing node that was visited
				Node prev = null;
				Node node = null;
				for (int i = existingSize - 1; i >= 0; i--)
				{
					node = nodePool.FindNode(existing[i]);
					if (node != null)
						break;
				}

				if (node == null)
				{
					node = query.LastBestNode;
				}

				//reverse the path
				do
				{
					Node next = nodePool.GetNodeAtIdx(node.ParentIdx);
					node.ParentIdx = nodePool.GetNodeIdx(prev);
					prev = node;
					node = next;
				}
				while (node != null);

				//store path
				node = prev;
				do
				{
					path[n++] = node.Id;
					if (n >= maxPath)
					{
						break;
					}

					node = nodePool.GetNodeAtIdx(node.ParentIdx);
				}
				while (node != null);
			}

			//reset query
			query = new QueryData();

			//remember to update the path length
			pathCount = n;

			return true;
		}

		public bool Raycast(NavPoint startPoint, Vector3 endPos, ref float t, ref Vector3 hitNormal, int[] path, ref int pathCount, int maxPath)
		{
			t = 0;
			pathCount = 0;

			//validate input
			if (startPoint.Polygon == 0 || !nav.IsValidPolyRef(startPoint.Polygon))
				return false;

			int curRef = startPoint.Polygon;
			Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
			int n = 0;

			hitNormal = new Vector3(0, 0, 0);

			while (curRef != 0)
			{
				//cast ray against current polygon
				MeshTile tile;
				Poly poly;
				nav.TryGetTileAndPolyByRefUnsafe(curRef, out tile, out poly);

				//collect vertices
				int nv = 0;
				for (int i = 0; i < poly.VertCount; i++)
				{
					verts[nv] = tile.Verts[poly.Verts[i]];
					nv++;
				}

				float tmin, tmax;
				int segMin, segMax;
				if (!Intersection.SegmentPoly2D(startPoint.Position, endPos, verts, nv, out tmin, out tmax, out segMin, out segMax))
				{
					//could not hit the polygon, keep the old t and report hit
					pathCount = n;
					return true;
				}

				//keep track of furthest t so far
				if (tmax > t)
					t = tmax;

				//store visited polygons
				if (n < maxPath)
					path[n++] = curRef;

				//ray end is completely inside the polygon
				if (segMax == -1)
				{
					t = float.MaxValue;
					pathCount = n;
					return true;
				}

				//follow neighbours
				int nextRef = 0;

				for (int i = poly.FirstLink; i != Link.Null; i = tile.Links[i].Next)
				{
					Link link = tile.Links[i];

					//find link which contains the edge
					if (link.Edge != segMax)
						continue;

					//get pointer to the next polygon
					MeshTile nextTile;
					Poly nextPoly;
					nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out nextTile, out nextPoly);

					//skip off-mesh connection
					if (nextPoly.PolyType == PolygonType.OffMeshConnection)
						continue;

					//if the link is internal, just return the ref
					if (link.Side == BoundarySide.Internal)
					{
						nextRef = link.Reference;
						break;
					}

					//if the link is at the tile boundary

					//check if the link spans the whole edge and accept
					if (link.BMin == 0 && link.BMax == 255)
					{
						nextRef = link.Reference;
						break;
					}

					//check for partial edge links
					int v0 = poly.Verts[link.Edge];
					int v1 = poly.Verts[(link.Edge + 1) % poly.VertCount];
					Vector3 left = tile.Verts[v0];
					Vector3 right = tile.Verts[v1];

					//check that the intersection lies inside the link portal
					if (link.Side == BoundarySide.PlusX || link.Side == BoundarySide.MinusX)
					{
						//calculate link size
						float s = 1.0f / 255.0f;
						float lmin = left.Z + (right.Z - left.Z) * (link.BMin * s);
						float lmax = left.Z + (right.Z - left.Z) * (link.BMax * s);
						if (lmin > lmax)
						{
							//swap
							float temp = lmin;
							lmin = lmax;
							lmax = temp;
						}

						//find z intersection
						float z = startPoint.Position.Z + (endPos.Z - startPoint.Position.Z) * tmax;
						if (z >= lmin && z <= lmax)
						{
							nextRef = link.Reference;
							break;
						}
					}
					else if (link.Side == BoundarySide.PlusZ || link.Side == BoundarySide.MinusZ)
					{
						//calculate link size
						float s = 1.0f / 255.0f;
						float lmin = left.X + (right.X - left.X) * (link.BMin * s);
						float lmax = left.X + (right.X - left.X) * (link.BMax * s);
						if (lmin > lmax)
						{
							//swap
							float temp = lmin;
							lmin = lmax;
							lmax = temp;
						}

						//find x intersection
						float x = startPoint.Position.X + (endPos.X - startPoint.Position.X) * tmax;
						if (x >= lmin && x <= lmax)
						{
							nextRef = link.Reference;
							break;
						}
					}
				}

				if (nextRef == 0)
				{
					//no neighbour, we hit a wall

					//calculate hit normal
					int a = segMax;
					int b = (segMax + 1) < nv ? segMax + 1 : 0;
					Vector3 va = verts[a];
					Vector3 vb = verts[b];
					float dx = vb.X - va.X;
					float dz = vb.Z - va.Z;
					hitNormal.X = dz;
					hitNormal.Y = 0;
					hitNormal.Z = -dx;
					hitNormal.Normalize();

					pathCount = n;
					return true;
				}

				//no hit, advance to neighbour polygon
				curRef = nextRef;
			}

			pathCount = n;

			return true;
		}

		/// <summary>
		/// Store polygons that are within a certain range from the current polygon
		/// </summary>
		/// <param name="centerPoint">Starting position</param>
		/// <param name="radius">Range to search within</param>
		/// <param name="resultRef">All the polygons within range</param>
		/// <param name="resultParent">Polygon's parents</param>
		/// <param name="resultCount">Number of polygons stored</param>
		/// <param name="maxResult">Maximum number of polygons allowed</param>
		/// <returns>True, unless input is invalid</returns>
		public bool FindLocalNeighbourhood(NavPoint centerPoint, float radius, int[] resultRef, int[] resultParent, ref int resultCount, int maxResult)
		{
			resultCount = 0;

			//validate input
			if (centerPoint.Polygon == 0 || !nav.IsValidPolyRef(centerPoint.Polygon))
				return false;

			int MAX_STACK = 48;
			Node[] stack = new Node[MAX_STACK];
			int nstack = 0;

			tinyNodePool.Clear();

			Node startNode = tinyNodePool.GetNode(centerPoint.Polygon);
			startNode.ParentIdx = 0;
			startNode.Id = centerPoint.Polygon;
			startNode.Flags = NodeFlags.Closed;
			stack[nstack++] = startNode;

			float radiusSqr = radius * radius;

			Vector3[] pa = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
			Vector3[] pb = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];

			int n = 0;
			if (n < maxResult)
			{
				resultRef[n] = startNode.Id;
				resultParent[n] = 0;
				++n;
			}

			while (nstack > 0)
			{
				//pop front
				Node curNode = stack[0];
				for (int i = 0; i < nstack - 1; i++)
					stack[i] = stack[i + 1];
				nstack--;

				//get poly and tile
				int curRef = curNode.Id;
				MeshTile curTile;
				Poly curPoly;
				nav.TryGetTileAndPolyByRefUnsafe(curRef, out curTile, out curPoly);

				for (int i = curPoly.FirstLink; i != Link.Null; i = curTile.Links[i].Next)
				{
					Link link = curTile.Links[i];
					int neighbourRef = link.Reference;

					//skip invalid neighbours
					if (neighbourRef == 0)
						continue;

					//skip if cannot allocate more nodes
					Node neighbourNode = tinyNodePool.GetNode(neighbourRef);
					if (neighbourNode == null)
						continue;

					//skip visited
					if ((neighbourNode.Flags & NodeFlags.Closed) != 0)
						continue;

					//expand to neighbour
					MeshTile neighbourTile;
					Poly neighbourPoly;
					nav.TryGetTileAndPolyByRefUnsafe(neighbourRef, out neighbourTile, out neighbourPoly);

					//skip off-mesh connections
					if (neighbourPoly.PolyType == PolygonType.OffMeshConnection)
						continue;

					//find edge and calculate distance to edge
					Vector3 va = new Vector3();
					Vector3 vb = new Vector3();
					if (!GetPortalPoints(curRef, curPoly, curTile, neighbourRef, neighbourPoly, neighbourTile, ref va, ref vb))
						continue;

					//if the circle is not touching the next polygon, skip it
					float tseg;
					float distSqr = Distance.PointToSegment2DSquared(ref centerPoint.Position, ref va, ref vb, out tseg);
					if (distSqr > radiusSqr)
						continue;

					//mark node visited
					neighbourNode.Flags |= NodeFlags.Closed;
					neighbourNode.ParentIdx = tinyNodePool.GetNodeIdx(curNode);

					//check that the polygon doesn't collide with existing polygons

					//collect vertices of the neighbour poly
					int npa = neighbourPoly.VertCount;
					for (int k = 0; k < npa; k++)
						pa[k] = neighbourTile.Verts[neighbourPoly.Verts[k]];

					bool overlap = false;
					for (int j = 0; j < n; j++)
					{
						int pastRef = resultRef[j];

						//connected polys do not overlap
						bool connected = false;
						for (int k = curPoly.FirstLink; k != Link.Null; k = curTile.Links[k].Next)
						{
							if (curTile.Links[k].Reference == pastRef)
							{
								connected = true;
								break;
							}
						}

						if (connected)
							continue;

						//potentially overlapping
						MeshTile pastTile;
						Poly pastPoly;
						nav.TryGetTileAndPolyByRefUnsafe(pastRef, out pastTile, out pastPoly);

						//get vertices and test overlap
						int npb = pastPoly.VertCount;
						for (int k = 0; k < npb; k++)
							pb[k] = pastTile.Verts[pastPoly.Verts[k]];

						if (Intersection.PolyPoly2D(pa, npa, pb, npb))
						{
							overlap = true;
							break;
						}
					}

					if (overlap)
						continue;

					//store poly
					if (n < maxResult)
					{
						resultRef[n] = neighbourRef;
						resultParent[n] = curRef;
						++n;
					}

					if (nstack < MAX_STACK)
					{
						stack[nstack++] = neighbourNode;
					}
				}
			}

			resultCount = n;

			return true;
		}

		/// <summary>
		/// Collect all the edges from a polygon.
		/// </summary>
		/// <param name="reference">The polygon reference</param>
		/// <param name="segmentVerts">Segment vertices</param>
		/// <param name="segmentRefs">The polygon reference containing the segment</param>
		/// <param name="segmentCount">The number of segments stored</param>
		/// <param name="maxSegments">The maximum number of segments allowed</param>
		/// <returns>True, unless the polygon reference is invalid</returns>
		public bool GetPolyWallSegments(int reference, Crowds.LocalBoundary.Segment[] segmentVerts, int[] segmentRefs, ref int segmentCount, int maxSegments)
		{
			segmentCount = 0;

			MeshTile tile;
			Poly poly;
			if (nav.TryGetTileAndPolyByRef(reference, out tile, out poly) == false)
				return false;

			int n = 0;
			int MAX_INTERVAL = 16;
			SegInterval[] ints = new SegInterval[MAX_INTERVAL];
			int nints;

			bool storePortals = segmentRefs.Length != 0;

			for (int i = 0, j = poly.VertCount - 1; i < poly.VertCount; j = i++)
			{
				//skip non-solid edges
				nints = 0;
				if ((poly.Neis[j] & Link.External) != 0)
				{
					//tile border
					for (int k = poly.FirstLink; k != Link.Null; k = tile.Links[k].Next)
					{
						Link link = tile.Links[k];
						if (link.Edge == j)
						{
							if (link.Reference != 0)
							{
								MeshTile neiTile;
								Poly neiPoly;
								nav.TryGetTileAndPolyByRefUnsafe(link.Reference, out neiTile, out neiPoly);
								InsertInterval(ints, ref nints, MAX_INTERVAL, link.BMin, link.BMax, link.Reference);
							}
						}
					}
				}
				else
				{
					//internal edge
					int neiRef = 0;
					if (poly.Neis[j] != 0)
					{
						int idx = poly.Neis[j] - 1;
						neiRef = nav.GetPolyRefBase(tile) | idx;
					}

					//if the edge leads to another polygon and portals are not stored, skip
					if (neiRef != 0 && !storePortals)
						continue;

					if (n < maxSegments)
					{
						Vector3 vj = tile.Verts[poly.Verts[j]];
						Vector3 vi = tile.Verts[poly.Verts[i]];
						segmentVerts[n].Start = vj;
						segmentVerts[n].End = vi;
						segmentRefs[n] = neiRef;
						n++; //could be n += 2, since segments have 2 vertices
					}

					continue;
				}

				//add sentinels
				InsertInterval(ints, ref nints, MAX_INTERVAL, -1, 0, 0);
				InsertInterval(ints, ref nints, MAX_INTERVAL, 255, 256, 0);

				//store segments
				Vector3 vj2 = tile.Verts[poly.Verts[j]];
				Vector3 vi2 = tile.Verts[poly.Verts[i]];
				for (int k = 1; k < nints; k++)
				{
					//portal segment
					if (storePortals && ints[k].Reference != 0)
					{
						float tmin = ints[k].TMin / 255.0f;
						float tmax = ints[k].TMax / 255.0f;
						if (n < maxSegments)
						{
							Vector3.Lerp(ref vj2, ref vi2, tmin, out segmentVerts[n].Start);
							Vector3.Lerp(ref vj2, ref vi2, tmax, out segmentVerts[n].End);
							segmentRefs[n] = ints[k].Reference;
							n++;
						}
					}

					//wall segment
					int imin = ints[k - 1].TMax;
					int imax = ints[k].TMin;
					if (imin != imax)
					{
						float tmin = imin / 255.0f;
						float tmax = imax / 255.0f;
						if (n < maxSegments)
						{
							Vector3.Lerp(ref vj2, ref vi2, tmin, out segmentVerts[n].Start);
							Vector3.Lerp(ref vj2, ref vi2, tmax, out segmentVerts[n].End);
							segmentRefs[n] = 0;
							n++; 
						}
					}
				}
			}

			segmentCount = n;

			return true;
		}

		/// <summary>
		/// Insert a segment into the array
		/// </summary>
		/// <param name="ints">The array of segments</param>
		/// <param name="nints">The number of segments</param>
		/// <param name="maxInts">The maximium number of segments allowed</param>
		/// <param name="tmin">Parameter t minimum</param>
		/// <param name="tmax">Parameter t maximum</param>
		/// <param name="reference">Polygon reference</param>
		public void InsertInterval(SegInterval[] ints, ref int nints, int maxInts, int tmin, int tmax, int reference)
		{
			if (nints + 1 > maxInts)
				return;

			//find insertion point
			int idx = 0;
			while (idx < nints)
			{
				if (tmax <= ints[idx].TMin)
					break;
				idx++;
			}

			//move current results
			if (nints - idx > 0)
			{
				for (int i = 0; i < nints - idx; i++)
					ints[idx + 1 + i] = ints[idx + i];
			}

			//store
			ints[idx].Reference = reference;
			ints[idx].TMin = tmin;
			ints[idx].TMax = tmax;
			nints++;
		}

		/// <summary>
		/// Get edge midpoint between two prolygons
		/// </summary>
		/// <param name="from">"From" polygon reference</param>
		/// <param name="fromPoly">"From" polygon data</param>
		/// <param name="fromTile">"From" mesh tile</param>
		/// <param name="to">"To" polygon reference</param>
		/// <param name="toPoly">"To" polygon data</param>
		/// <param name="toTile">"To" mesh tile</param>
		/// <param name="mid">Edge midpoint</param>
		/// <returns>True, if midpoint found. False, if otherwise.</returns>
		public bool GetEdgeMidPoint(int from, Poly fromPoly, MeshTile fromTile, int to, Poly toPoly, MeshTile toTile, ref Vector3 mid)
		{
			Vector3 left = new Vector3();
			Vector3 right = new Vector3();
			if (!GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right))
				return false;

			mid = (left + right) * 0.5f;

			return true;
		}

		/// <summary>
		/// Find points on the left and right side.
		/// </summary>
		/// <param name="from">"From" polygon reference</param>
		/// <param name="to">"To" polygon reference</param>
		/// <param name="left">Point on the left side</param>
		/// <param name="right">Point on the right side</param>
		/// <param name="fromType">Polygon type of "From" polygon</param>
		/// <param name="toType">Polygon type of "To" polygon</param>
		/// <returns>True, if points found. False, if otherwise.</returns>
		public bool GetPortalPoints(int from, int to, ref Vector3 left, ref Vector3 right, ref PolygonType fromType, ref PolygonType toType)
		{
			MeshTile fromTile;
			Poly fromPoly;
			if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false)
				return false;
			fromType = fromPoly.PolyType;

			MeshTile toTile;
			Poly toPoly;
			if (nav.TryGetTileAndPolyByRef(to, out toTile, out toPoly) == false)
				return false;
			toType = toPoly.PolyType;

			return GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right);
		}

		/// <summary>
		/// Find points on the left and right side.
		/// </summary>
		/// <param name="from">"From" polygon reference</param>
		/// <param name="fromPoly">"From" polygon data</param>
		/// <param name="fromTile">"From" mesh tile</param>
		/// <param name="to">"To" polygon reference</param>
		/// <param name="toPoly">"To" polygon data</param>
		/// <param name="toTile">"To" mesh tile</param>
		/// <param name="left">Resulting point on the left side</param>
		/// <param name="right">Resulting point on the right side</param>
		/// <returns>True, if points found. False, if otherwise.</returns>
		public bool GetPortalPoints(int from, Poly fromPoly, MeshTile fromTile, int to, Poly toPoly, MeshTile toTile, ref Vector3 left, ref Vector3 right)
		{
			//find the link that points to the 'to' polygon
			Link link = null;
			for (int i = fromPoly.FirstLink; i != Link.Null; i = fromTile.Links[i].Next)
			{
				if (fromTile.Links[i].Reference == to)
				{
					link = fromTile.Links[i];
					break;
				}
			}

			if (link == null)
				return false;

			//handle off-mesh connections
			if (fromPoly.PolyType == PolygonType.OffMeshConnection)
			{
				//find link that points to first vertex
				for (int i = fromPoly.FirstLink; i != Link.Null; i = fromTile.Links[i].Next)
				{
					if (fromTile.Links[i].Reference == to)
					{
						int v = fromTile.Links[i].Edge;
						left = fromTile.Verts[fromPoly.Verts[v]];
						right = fromTile.Verts[fromPoly.Verts[v]];
						return true;
					}
				}

				return false;
			}

			if (toPoly.PolyType == PolygonType.OffMeshConnection)
			{
				//find link that points to first vertex
				for (int i = toPoly.FirstLink; i != Link.Null; i = toTile.Links[i].Next)
				{
					if (toTile.Links[i].Reference == from)
					{
						int v = toTile.Links[i].Edge;
						left = toTile.Verts[toPoly.Verts[v]];
						right = toTile.Verts[toPoly.Verts[v]];
						return true;
					}
				}

				return false;
			}

			//find portal vertices
			int v0 = fromPoly.Verts[link.Edge];
			int v1 = fromPoly.Verts[(link.Edge + 1) % fromPoly.VertCount];
			left = fromTile.Verts[v0];
			right = fromTile.Verts[v1];

			//if the link is at the tile boundary, clamp the vertices to tile width
			if (link.Side != BoundarySide.Internal)
			{
				//unpack portal limits
				if (link.BMin != 0 || link.BMax != 255)
				{
					float s = 1.0f / 255.0f;
					float tmin = link.BMin * s;
					float tmax = link.BMax * s;
					left = Vector3.Lerp(fromTile.Verts[v0], fromTile.Verts[v1], tmin);
					right = Vector3.Lerp(fromTile.Verts[v0], fromTile.Verts[v1], tmax);
				}
			}

			return true;
		}

		/// <summary>
		/// Given a point on the polygon, find the closest point
		/// </summary>
		/// <param name="reference">Polygon reference</param>
		/// <param name="pos">Given point</param>
		/// <param name="closest">Resulting closest point</param>
		/// <returns>True, if point found. False, if otherwise.</returns>
		public bool ClosestPointOnPoly(int reference, Vector3 pos, ref Vector3 closest)
		{
			if (nav == null)
				return false;

			MeshTile tile;
			Poly poly;

			if (nav.TryGetTileAndPolyByRef(reference, out tile, out poly) == false)
				return false;

			if (tile == null)
				return false;

			tile.ClosestPointOnPoly(poly, pos, ref closest);
			return true;
		}

		/// <summary>
		/// Given a point on the polygon, find the closest point
		/// </summary>
		/// <param name="reference">Polygon reference</param>
		/// <param name="pos">Current position</param>
		/// <param name="closest">Resulting closest position</param>
		/// <param name="posOverPoly">Determines whether the position can be found on the polygon</param>
		/// <returns>True, if the closest point is found. False, if otherwise.</returns>
		public bool ClosestPointOnPoly(int reference, Vector3 pos, out Vector3 closest, out bool posOverPoly)
		{
			posOverPoly = false;
			closest = Vector3.Zero;

			MeshTile tile;
			Poly poly;
			if (!nav.TryGetTileAndPolyByRef(reference, out tile, out poly))
				return false;
			if (tile == null)
				return false;

			if (poly.PolyType == PolygonType.OffMeshConnection)
			{
				Vector3 v0 = tile.Verts[poly.Verts[0]];
				Vector3 v1 = tile.Verts[poly.Verts[1]];
				float d0 = (pos - v0).Length();
				float d1 = (pos - v1).Length();
				float u = d0 / (d0 + d1);
				closest = Vector3.Lerp(v0, v1, u);
				return true;
			}

			int indexPoly = 0;
			for (int i = 0; i < tile.Polys.Length; i++)
			{
				if (tile.Polys[i] == poly)
				{
					indexPoly = i;
					break;
				}
			}

			PolyMeshDetail.MeshData pd = tile.DetailMeshes[indexPoly];

			//Clamp point to be inside the polygon
			Vector3[] verts = new Vector3[PathfindingCommon.VERTS_PER_POLYGON];
			float[] edgeDistance = new float[PathfindingCommon.VERTS_PER_POLYGON];
			float[] edgeT = new float[PathfindingCommon.VERTS_PER_POLYGON];
			int numPolyVerts = poly.VertCount;
			for (int i = 0; i < numPolyVerts; i++)
				verts[i] = tile.Verts[poly.Verts[i]];

			closest = pos;
			if (!Distance.PointToPolygonEdgeSquared(pos, verts, numPolyVerts, edgeDistance, edgeT))
			{
				//Point is outside the polygon
				//Clamp to nearest edge
				float minDistance = float.MaxValue;
				int minIndex = -1;
				for (int i = 0; i < numPolyVerts; i++)
				{
					if (edgeDistance[i] < minDistance)
					{
						minDistance = edgeDistance[i];
						minIndex = i;
					}
				}

				Vector3 va = verts[minIndex];
				Vector3 vb = verts[(minIndex + 1) % numPolyVerts];
				closest = Vector3.Lerp(va, vb, edgeT[minIndex]);
			}
			else
			{
				posOverPoly = false;
			}

			//find height at the location
			for (int j = 0; j < tile.DetailMeshes[indexPoly].TriangleCount; j++)
			{
				PolyMeshDetail.TriangleData t = tile.DetailTris[pd.TriangleIndex + j];
				Vector3 va, vb, vc;

				if (t.VertexHash0 < poly.VertCount)
					va = tile.Verts[poly.Verts[t.VertexHash0]];
				else
					va = tile.DetailVerts[pd.VertexIndex + (t.VertexHash0 - poly.VertCount)];

				if (t.VertexHash1 < poly.VertCount)
					vb = tile.Verts[poly.Verts[t.VertexHash1]];
				else
					vb = tile.DetailVerts[pd.VertexIndex + (t.VertexHash1 - poly.VertCount)];

				if (t.VertexHash2 < poly.VertCount)
					vc = tile.Verts[poly.Verts[t.VertexHash2]];
				else
					vc = tile.DetailVerts[pd.VertexIndex + (t.VertexHash2 - poly.VertCount)];

				float h;
				if (Distance.PointToTriangle(pos, va, vb, vc, out h))
				{
					closest.Y = h;
					break;
				}
			}

			return true;
		}

		/// <summary>
		/// Given a point on a polygon, find the closest point which lies on the polygon boundary.
		/// </summary>
		/// <param name="reference">Polygon reference</param>
		/// <param name="pos">Current position</param>
		/// <param name="closest">Resulting closest point</param>
		/// <returns>True, if the closest point is found. False, if otherwise.</returns>
		public bool ClosestPointOnPolyBoundary(int reference, Vector3 pos, ref Vector3 closest)
		{
			MeshTile tile;
			Poly poly;
			if (nav.TryGetTileAndPolyByRef(reference, out tile, out poly) == false)
				return false;

			tile.ClosestPointOnPolyBoundary(poly, pos, out closest);
			return true;
		}

		/// <summary>
		/// Add a vertex to the straight path.
		/// </summary>
		/// <param name="pos"></param>
		/// <param name="flags"></param>
		/// <param name="reference"></param>
		/// <param name="straightPath">An array of points on the straight path</param>
		/// <param name="straightPathFlags">An array of flags</param>
		/// <param name="straightPathRefs">An array of polygon references</param>
		/// <param name="straightPathCount">The number of points on the path</param>
		/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
		/// <returns>True, if end of path hasn't been reached yet and path isn't full. False, if otherwise.</returns>
		public bool AppendVertex(Vector3 pos, int flags, int reference, Vector3[] straightPath, int[] straightPathFlags, int[] straightPathRefs, ref int straightPathCount, int maxStraightPath)
		{
			if (straightPathCount > 0 && straightPath[straightPathCount - 1] == pos)
			{
				//the vertices are equal
				//update flags and polys
				if (straightPathFlags.Length != 0)
					straightPathFlags[straightPathCount - 1] = flags;
				
				if (straightPathRefs.Length != 0)
					straightPathRefs[straightPathCount - 1] = reference;
			}
			else
			{
				//append new vertex
				straightPath[straightPathCount] = pos;
				
				if (straightPathFlags.Length != 0)
					straightPathFlags[straightPathCount] = flags;
				
				if (straightPathRefs.Length != 0)
					straightPathRefs[straightPathCount] = reference;
				
				straightPathCount++;

				if (flags == PathfindingCommon.STRAIGHTPATH_END || straightPathCount >= maxStraightPath)
				{
					return false;
				}
			}

			return true;
		}

		/// <summary>
		/// Update the vertices on the straight path
		/// </summary>
		/// <param name="startIdx">Original path's starting index</param>
		/// <param name="endIdx">Original path's end index</param>
		/// <param name="endPos">The end position</param>
		/// <param name="path">The original path of polygon references</param>
		/// <param name="straightPath">An array of points on the straight path</param>
		/// <param name="straightPathFlags">An array of flags</param>
		/// <param name="straightPathRefs">An array of polygon references</param>
		/// <param name="straightPathCount">The number of points on the path</param>
		/// <param name="maxStraightPath">The maximum length allowed for the straight path</param>
		/// <param name="options">Options flag</param>
		/// <returns></returns>
		public bool AppendPortals(int startIdx, int endIdx, Vector3 endPos, int[] path, Vector3[] straightPath, int[] straightPathFlags, int[] straightPathRefs, ref int straightPathCount, int maxStraightPath, PathBuildFlags options)
		{
			Vector3 startPos = straightPath[straightPathCount - 1];

			//append or update last vertex
			bool stat = false;
			for (int i = startIdx; i < endIdx; i++)
			{
				//calculate portal
				int from = path[i];
				MeshTile fromTile;
				Poly fromPoly;
				if (nav.TryGetTileAndPolyByRef(from, out fromTile, out fromPoly) == false)
					return false;

				int to = path[i + 1];
				MeshTile toTile;
				Poly toPoly;
				if (nav.TryGetTileAndPolyByRef(to, out toTile, out toPoly) == false)
					return false;

				Vector3 left = new Vector3();
				Vector3 right = new Vector3();
				if (GetPortalPoints(from, fromPoly, fromTile, to, toPoly, toTile, ref left, ref right) == false)
					break;

				if ((options & PathBuildFlags.AreaCrossingVertices) != 0)
				{
					//skip intersection if only area crossings are requested
					if (fromPoly.Area == toPoly.Area)
						continue;
				}

				//append intersection
				float s, t;
				if (Intersection.SegmentSegment2D(ref startPos, ref endPos, ref left, ref right, out s, out t))
				{
					Vector3 pt = Vector3.Lerp(left, right, t);

					stat = AppendVertex(pt, 0, path[i + 1], straightPath, straightPathFlags, straightPathRefs, ref straightPathCount, maxStraightPath);

					if (stat != true)
						return true;
				}
			}

			return true;
		}

		/// <summary>
		/// Return false if the provided position is outside the xz-bounds.
		/// </summary>
		/// <param name="reference">Polygon reference</param>
		/// <param name="pos">Current position</param>
		/// <param name="height">Resulting polygon height</param>
		/// <returns>True, if height found. False, if otherwise.</returns>
		public bool GetPolyHeight(int reference, Vector3 pos, ref float height)
		{
			if (nav == null)
				return false;

			MeshTile tile;
			Poly poly;
			if (!nav.TryGetTileAndPolyByRef(reference, out tile, out poly))
				return false;

			//off-mesh connections don't have detail polygons
			if (poly.PolyType == PolygonType.OffMeshConnection)
			{
				Vector3 closest;
				tile.ClosestPointOnPolyOffMeshConnection(poly, pos, out closest);
				height = closest.Y;
				return true;
			}
			else
			{
				int indexPoly = 0;
				for (int i = 0; i < tile.Polys.Length; i++)
				{
					if (tile.Polys[i] == poly)
					{
						indexPoly = i;
						break;
					}
				}

				float h = 0;
				if (tile.ClosestHeight(indexPoly, pos, out h))
				{
					height = h;
					return true;
				}
			}

			return false;
		}

		/// <summary>
		/// Find the nearest poly within a certain range.
		/// </summary>
		/// <param name="center">Center.</param>
		/// <param name="extents">Extents.</param>
		/// <returns>The neareast point.</returns>
		public NavPoint FindNearestPoly(Vector3 center, Vector3 extents)
		{
			NavPoint result;
			this.FindNearestPoly(ref center, ref extents, out result);
			return result;
		}

		/// <summary>
		/// Find the nearest poly within a certain range.
		/// </summary>
		/// <param name="center">Center.</param>
		/// <param name="extents">Extents.</param>
		/// <param name="nearestPt">The neareast point.</param>
		public void FindNearestPoly(ref Vector3 center, ref Vector3 extents, out NavPoint nearestPt)
		{
			nearestPt = NavPoint.Null;

			//TODO error state?

			// Get nearby polygons from proximity grid.
			List<int> polys = new List<int>(128);
			if (!QueryPolygons(ref center, ref extents, polys))
				throw new InvalidOperationException("no nearby polys?");

			float nearestDistanceSqr = float.MaxValue;
			for (int i = 0; i < polys.Count; i++) 
			{
				int reference = polys[i];
				Vector3 closestPtPoly;
				bool posOverPoly;
				ClosestPointOnPoly(reference, center, out closestPtPoly, out posOverPoly);

				// If a point is directly over a polygon and closer than
				// climb height, favor that instead of straight line nearest point.
				Vector3 diff = center - closestPtPoly;
				float d = 0;
				if (posOverPoly)
				{
					MeshTile tile;
					Poly poly;
					nav.TryGetTileAndPolyByRefUnsafe(polys[i], out tile, out poly);
					d = Math.Abs(diff.Y) - tile.Header.WalkableClimb;
					d = d > 0 ? d * d : 0;
				}
				else
				{
					d = diff.LengthSquared();
				}

				if (d < nearestDistanceSqr)
				{
					nearestDistanceSqr = d;
					nearestPt = new NavPoint(reference, closestPtPoly);
				}
			}
		}

		/// <summary>
		/// Finds nearby polygons within a certain range.
		/// </summary>
		/// <param name="center">The starting point</param>
		/// <param name="extent">The range to search within</param>
		/// <param name="polys">A list of polygons</param>
		/// <returns>True, if successful. False, if otherwise.</returns>
		public bool QueryPolygons(ref Vector3 center, ref Vector3 extent, List<int> polys)
		{
			Vector3 bmin = center - extent;
			Vector3 bmax = center + extent;

			int minx, miny, maxx, maxy;
			nav.CalcTileLoc(ref bmin, out minx, out miny);
			nav.CalcTileLoc(ref bmax, out maxx, out maxy);

			MeshTile[] neis = new MeshTile[32];
			
			BBox3 bounds = new BBox3(bmin, bmax);
			int n = 0;
			for (int y = miny; y <= maxy; y++)
			{
				for (int x = minx; x <= maxx; x++)
				{
					int nneis = nav.GetTilesAt(x, y, neis);
					for (int j = 0; j < nneis; j++)
					{
						n += nav.QueryPolygonsInTile(neis[j], bounds, polys);
						if (n >= polys.Capacity) 
						{
							return true;
						}
					}
				}
			}

			return polys.Count != 0;
		}

		public bool IsValidPolyRef(int reference)
		{
			MeshTile tile;
			Poly poly;
			bool status = nav.TryGetTileAndPolyByRef(reference, out tile, out poly);
			if (status == false)
				return false;
			return true;
		}

		public bool IsInOpenList(Node node)
		{
			return (node.Flags & NodeFlags.Open) != 0;
		}

		public bool IsInClosedList(Node node)
		{
			return (node.Flags & NodeFlags.Closed) != 0;
		}

		public void SetNodeFlagOpen(ref Node node)
		{
			node.Flags |= NodeFlags.Open;
		}

		public void SetNodeFlagClosed(ref Node node)
		{
			node.Flags &= ~NodeFlags.Open;
			node.Flags |= NodeFlags.Closed;
		}

		public NodeFlags RemoveNodeFlagClosed(Node node)
		{
			return node.Flags & ~NodeFlags.Closed;
		}

		private struct QueryData
		{
			public bool Status;
			public Node LastBestNode;
			public float LastBestNodeCost;
			public int StartRef, EndRef;
			public Vector3 StartPos, EndPos;
		}

		public struct SegInterval
		{
			public int Reference;
			public int TMin, TMax;
		}
	}
}