using System.Collections.Generic;
using Pathfinding.Pooling;
using Unity.Mathematics;
using UnityEngine;
using UnityEngine.Profiling;

namespace Pathfinding {
	/// <summary>
	/// Simplifies a path on a grid graph using a string pulling algorithm.
	/// This is based on a paper called "Toward a String-Pulling Approach to Path Smoothing on Grid Graphs",
	/// with some optimizations as well as fixes for some edge cases that the paper didn't handle.
	///
	/// The result is conceptually similar to the well known funnel string pulling algorithm for navmesh graphs
	/// but it uses a different algorithm.
	///
	/// This class is used by the <see cref="FunnelModifier"/> on grid graphs.
	///
	/// See: <see cref="Funnel"/>
	/// See: <see cref="FunnelModifier"/>
	/// See: article: https://ojs.aaai.org/index.php/SOCS/article/view/18541
	/// </summary>
	public static class GridStringPulling {
		/// <summary>
		///         Z
		///         |
		///         |
		///
		///      3     2
		///       \ | /
		/// --    - X -    ----- X
		///       / | \
		///      0     1
		///
		///         |
		///         |
		/// </summary>
		static int2[] directionToCorners = new int2[] {
			new int2(0, 0),
			new int2(FixedPrecisionScale, 0),
			new int2(FixedPrecisionScale, FixedPrecisionScale),
			new int2(0, FixedPrecisionScale),
		};

		static long Cross (int2 lhs, int2 rhs) {
			return (long)lhs.x*(long)rhs.y - (long)lhs.y*(long)rhs.x;
		}

		static long Dot (int2 a, int2 b) {
			return (long)a.x*(long)b.x + (long)a.y*(long)b.y;
		}

		static bool RightOrColinear (int2 a, int2 b, int2 p) {
			return (long)(b.x - a.x) * (long)(p.y - a.y) - (long)(p.x - a.x) * (long)(b.y - a.y) <= 0;
		}

		static int2 Perpendicular (int2 v) {
			return new int2(-v.y, v.x);
		}

		struct TriangleBounds {
			int2 d1, d2, d3;
			long t1, t2, t3;

			public TriangleBounds(int2 p1, int2 p2, int2 p3) {
				if (RightOrColinear(p1, p2, p3)) {
					var tmp = p3;
					p3 = p1;
					p1 = tmp;
				}
				d1 = Perpendicular(p2 - p1);
				d2 = Perpendicular(p3 - p2);
				d3 = Perpendicular(p1 - p3);
				t1 = Dot(d1, p1);
				t2 = Dot(d2, p2);
				t3 = Dot(d3, p3);
			}

			public bool Contains (int2 p) {
				return Dot(d1, p) >= t1 && Dot(d2, p) >= t2 && Dot(d3, p) >= t3;
			}
		}

		const int FixedPrecisionScale = 1024;

		static int2 ToFixedPrecision (Vector2 p) {
			return new int2(math.round(new float2(p)*FixedPrecisionScale));
		}

		static Vector2 FromFixedPrecision (int2 p) {
			return (Vector2)(((float2)p) * (1.0f/FixedPrecisionScale));
		}

		/// <summary>Returns which side of the line a - b that p lies on</summary>
		static Side Side2D (int2 a, int2 b, int2 p) {
			var s = Cross(b-a, p-a);

			return s > 0 ? Side.Left : (s < 0 ? Side.Right : Side.Colinear);
		}

		static Unity.Profiling.ProfilerMarker marker1 = new Unity.Profiling.ProfilerMarker("Linecast hit");
		static Unity.Profiling.ProfilerMarker marker2 = new Unity.Profiling.ProfilerMarker("Linecast success");
		static Unity.Profiling.ProfilerMarker marker3 = new Unity.Profiling.ProfilerMarker("Trace");
		static Unity.Profiling.ProfilerMarker marker4 = new Unity.Profiling.ProfilerMarker("Neighbours");
		static Unity.Profiling.ProfilerMarker marker5 = new Unity.Profiling.ProfilerMarker("Re-evaluate linecast");
		static Unity.Profiling.ProfilerMarker marker6 = new Unity.Profiling.ProfilerMarker("Init");
		static Unity.Profiling.ProfilerMarker marker7 = new Unity.Profiling.ProfilerMarker("Initloop");

		/// <summary>
		/// Intersection length of the given segment with a square of size Int3.Precision centered at nodeCenter.
		/// The return value is between 0 and sqrt(2).
		/// </summary>
		public static float IntersectionLength (int2 nodeCenter, int2 segmentStart, int2 segmentEnd) {
			// TODO: Calculations can be hoisted
			var invNormal = math.rcp((float2)(segmentEnd - segmentStart));
			var normalMagn = math.length((float2)(segmentEnd - segmentStart));

			float tmin = float.NegativeInfinity, tmax = float.PositiveInfinity;

			var normal = segmentEnd - segmentStart;
			var bmin = nodeCenter; // - new int2(Int3.Precision/2, Int3.Precision/2);
			var bmax = nodeCenter + new int2(FixedPrecisionScale, FixedPrecisionScale);

			if (normal.x != 0.0) {
				float tx1 = (bmin.x - segmentStart.x)*invNormal.x;
				float tx2 = (bmax.x - segmentStart.x)*invNormal.x;

				tmin = math.max(tmin, math.min(tx1, tx2));
				tmax = math.min(tmax, math.max(tx1, tx2));
			} else if (segmentStart.x < bmin.x || segmentStart.x > bmax.x) {
				return 0.0f;
			}

			if (normal.y != 0.0) {
				float ty1 = (bmin.y - segmentStart.y)*invNormal.y;
				float ty2 = (bmax.y - segmentStart.y)*invNormal.y;

				tmin = math.max(tmin, math.min(ty1, ty2));
				tmax = math.min(tmax, math.max(ty1, ty2));
			} else if (segmentStart.y < bmin.y || segmentStart.y > bmax.y) {
				return 0.0f;
			}

			tmin = math.max(0, tmin);
			tmax = math.min(1, tmax);
			return math.max(tmax - tmin, 0)*normalMagn*(1.0f/FixedPrecisionScale);
		}

		internal static void TestIntersectionLength () {
			var s = FixedPrecisionScale;

			UnityEngine.Assertions.Assert.AreApproximatelyEqual(math.sqrt(2), IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(2*s, 2*s)));
			UnityEngine.Assertions.Assert.AreApproximatelyEqual(0.0f, IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(0, 0)));
			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(-1*s, s+1), new int2(2*s, s+1)));
			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(2*s, s), new int2(-1*s, s)));

			// All sides of the square should be included
			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s), new int2(s+s, s)));
			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s), new int2(s+s, s+s)));
			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s+s), new int2(s, s+s)));
			UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s+s), new int2(s, s)));
		}

		/// <summary>
		/// Cost of moving across all the nodes in the list, along the given segment.
		/// It is assumed that the segment intersects the nodes. Any potentially intersecting nodes that are not part of the list will be ignored.
		/// </summary>
		static uint LinecastCost (List<GraphNode> trace, int2 segmentStart, int2 segmentEnd, GridGraph gg, ref TraversalCosts traversalCosts) {
			// Check the cost of the segment compared to not taking this "shortcut"
			uint cost = 0;

			for (int k = 0; k < trace.Count; k++) {
				var node = trace[k] as GridNodeBase;
				if (k > 0) cost += traversalCosts.GetConnectionCost(trace[k-1], trace[k]);
				cost += (uint)(traversalCosts.GetTraversalCostMultiplier(node) * gg.nodeSize * Int3.Precision * IntersectionLength(new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid)*FixedPrecisionScale, segmentStart, segmentEnd));
			}
			return cost;
		}

		enum PredicateFailMode {
			Undefined,
			Turn,
			LinecastObstacle,
			LinecastCost,
			ReachedEnd,
		}

		/// <summary>
		/// Simplifies a path on a grid graph using a string pulling algorithm.
		/// See the class documentation for more details.
		/// </summary>
		/// <param name="pathNodes">A list of input nodes. Only the slice of nodes from nodeStartIndex to nodeEndIndex (inclusive) will be used. These must all be of type GridNodeBase and must form a path (i.e. each node must be a neighbor to the next one in the list).</param>
		/// <param name="nodeStartIndex">The index in pathNodes to start from.</param>
		/// <param name="nodeEndIndex">The last index in pathNodes that is used.</param>
		/// <param name="startPoint">A more exact start point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface).</param>
		/// <param name="endPoint">A more exact end point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface).</param>
		/// <param name="traversalConstraint">Can be used to prevent the linecast from traversing some nodes. Use \reflink{TraversalConstraint.None} to not apply any constraints. It is assumed that all nodes in pathNodes pass this filter.</param>
		/// <param name="traversalCosts">Can be used to specify how much it costs to traverse each node. Sometimes the geometrically shortest path is not the best path. The algorithm will still discard any simplifications which don't make the path less costly.</param>
		/// <param name="maxCorners">If you only need the first N points of the result, you can specify that here, to avoid unnecessary work.</param>
		public static List<Vector3> Calculate (List<GraphNode> pathNodes, int nodeStartIndex, int nodeEndIndex, Vector3 startPoint, Vector3 endPoint, ref TraversalCosts traversalCosts, ref TraversalConstraint traversalConstraint, int maxCorners = int.MaxValue) {
			Profiler.BeginSample("Funnel");
			marker6.Begin();
			// A list of indices into the arrays defined below.
			// Each index represents a point. But it's more convenient to use indices here and keep all the data separately (also probably faster).
			var outputPath = ListPool<int>.Claim();
			outputPath.Add(0);

			var numInputNodes = nodeEndIndex - nodeStartIndex + 1;
			var gg = pathNodes[nodeStartIndex].Graph as GridGraph;
			var trace = ListPool<GraphNode>.Claim();
			var turn = Side.Colinear;
			int counter = 0;

			// We will add two points, see comments inside the loop.
			// We may end up adding even more points later, therefore we get arrays that are a bit larger than we need for the initial path.
			numInputNodes += 2;
			int numPoints = numInputNodes;
			var nodes = ArrayPool<GridNodeBase>.Claim(numPoints*2);
			var points = ArrayPool<int2>.Claim(numPoints*2);
			var normalizedPoints = ArrayPool<int2>.Claim(numPoints*2);
			var costs = ArrayPool<uint>.Claim(numPoints*2);

			marker7.Begin();
			uint costSoFar = 0;
			// Go through all points and store all relevant data we need about them
			for (int j = 0; j < numInputNodes; j++) {
				// After the start-end modifier has adjusted the endpoints of the path, the line from the start point to the center of the second node in the path
				// might not actually have line of sight.
				// Assume the path starts at N1 with a diagonal move to node N2.
				// The start-end modifier adjusts the start point of the path to point S.
				// This causes the path to cut the corner to the unwalkable node in the bottom right.
				// ┌─────────┬────────┐
				// │         │        │
				// │   N2    │        │
				// │     \   │        │
				// │      \  │        │
				// ├───────\─┼────────┤
				// │########\│        │
				// │#########│S  N1   │
				// │#########│        │
				// │#########│        │
				// └─────────┴────────┘
				// We can solve this case by making the path go from S to N1 and then to N2 instead of directly from S to N2.
				// We also do the same thing for the end of the path.
				// The clamping and indexing here is essentially equivalent to one insert at the beginning of the arrays and one at the end.
				var node = nodes[j] = pathNodes[math.clamp(nodeStartIndex + j-1, nodeStartIndex, nodeEndIndex)] as GridNodeBase;
				var gridCoordinates = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid);
				var point = gridCoordinates * FixedPrecisionScale;
				int2 normalized;
				if (j == 0) {
					normalized = ToFixedPrecision(node.NormalizePoint(startPoint));
					normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale));
				} else if (j == numInputNodes - 1) {
					normalized = ToFixedPrecision(node.NormalizePoint(endPoint));
					normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale));
				} else {
					normalized = new int2(FixedPrecisionScale/2, FixedPrecisionScale/2);
				}
				points[j] = point + normalized;
				normalizedPoints[j] = normalized;
				if (j > 0) {
					// Calculate the cost of moving along the original path
					// Note: For the first/last nodes, we may have duplicate nodes with different normalized points.
					if (nodes[j-1] != nodes[j]) costSoFar += traversalCosts.GetConnectionCost(nodes[j-1], nodes[j]);
					costSoFar += (uint)((float)traversalCosts.GetTraversalCostMultiplier(nodes[j-1]) * gg.nodeSize * Int3.Precision * IntersectionLength(new int2(nodes[j-1].XCoordinateInGrid, nodes[j-1].ZCoordinateInGrid)*FixedPrecisionScale, points[j-1], points[j]));
					costSoFar += (uint)((float)traversalCosts.GetTraversalCostMultiplier(nodes[j]) * gg.nodeSize * Int3.Precision * IntersectionLength(gridCoordinates*FixedPrecisionScale, points[j-1], points[j]));
				}
				costs[j] = costSoFar;
			}
			marker7.End();

			// We know that there is line of sight from the first point to the second point in the path.
			var lastSuccessfulStart = 0;
			var lastSuccessfulEnd = 1;
			marker6.End();

			int i = 1;
			while (true) {
				if (i >= numInputNodes) {
					// We are done, add the last point
					outputPath.Add(numInputNodes-1);
					break;
				}
				if (outputPath.Count >= maxCorners) {
					// We are done with the partial result
					break;
				}

				counter++;
				if (counter > 10000) {
					Debug.LogError("Inf loop");
					break;
				}

				// In the paper, they just use a straight forward loop over the input path.
				// However, it is better for performance to use a binary search to figure out the next time we need to do something.
				// We only need an 'i' which succeeds and 'i+1' which fails.
				// The success in this case is defined by the predicate below. We only need to do stuff if that returns true.
				var last = outputPath[outputPath.Count-1];
				var normalizedLast = normalizedPoints[last];
				var prev = outputPath.Count > 1 ? outputPath[outputPath.Count-2] : -1;
				var nodeLast = nodes[last];
				var upperBound = numInputNodes - i - 1;

				// Lower and upper bounds for the binary search
				int mn = 0;
				// It is reasonable that most paths can be simplified at least a bit. Assume that seeing 4 or more nodes ahead is common.
				int mx = math.min(4, upperBound);
				var mxFailMode = PredicateFailMode.Undefined;
				uint mxLinecastCost = 0;

				// The calculations are not perfectly accurate. Allow the shortcut's cost to be a tiny bit higher.
				const uint COST_FUDGE = 5;

				GridHitInfo hit;
				// First fire off linecasts to nodes exponentially further away until the predicate returns true.
				while (true) {
					var idx = i + mx;

					var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn;
					if (turnPredicate) {
						mxFailMode = PredicateFailMode.Turn;
						break;
					} else {
						trace.Clear();
						if (idx == last+1) {
							// Linecast and cost check should in theory always succeed if we are just checking the next node in the path
							// However, in practice, it may pick the "wrong" node in edge cases when going through diagonal connections (could be more costly).
							// So for performance, and robustness, we just assume that the linecast will succeed.
						} else if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, ref traversalConstraint, trace)) {
							mxFailMode = PredicateFailMode.LinecastObstacle;
							break;
						} else {
							var cost = LinecastCost(trace, points[last], points[idx], gg, ref traversalCosts);
							if (cost > costs[idx] - costs[last] + COST_FUDGE) {
								// The "shortcut" had such a high penalty that it's not worth taking it
								mxFailMode = PredicateFailMode.LinecastCost;
								mxLinecastCost = cost;
								break;
							}
						}
					}

					if (mx < upperBound) {
						mn = mx;
						mx = math.min(mx*2, upperBound);
					} else {
						mxFailMode = PredicateFailMode.ReachedEnd;
						break;
					}
				}

				if (mxFailMode == PredicateFailMode.ReachedEnd) {
					// Reached final node without any hits, we can stop here
					outputPath.Add(numInputNodes-1);
					break;
				}

				// Run a standard binary search
				while (mx > mn + 1) {
					int mid = (mn+mx)/2;
					int idx = i + mid;

					var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn;
					bool pred = turnPredicate;
					if (turnPredicate) {
						mxFailMode = PredicateFailMode.Turn;
					} else {
						trace.Clear();
						if (idx == last+1) {
							// Linecast and cost check should in theory always succeed if we are just checking the next node in the path
							// However, in practice, it may pick the "wrong" node in edge cases when going through diagonal connections (could be more costly).
							// So for performance, and robustness, we just assume that the linecast will succeed.
						} else if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, ref traversalConstraint, trace)) {
							mxFailMode = PredicateFailMode.LinecastObstacle;
							pred = true;
						} else {
							var cost = LinecastCost(trace, points[last], points[idx], gg, ref traversalCosts);
							if (cost > costs[idx] - costs[last] + COST_FUDGE) {
								// The "shortcut" had such a high penalty that it's not worth taking it
								mxFailMode = PredicateFailMode.LinecastCost;
								mxLinecastCost = cost;
								pred = true;
							}
						}
					}

					if (pred) {
						mx = mid;
					} else {
						mn = mid;
					}
				}

				// i+mn is now a succeeding index, and i+mn+1 (or i+mx) is a failing index
				if (mn > 0) {
					lastSuccessfulStart = last;
					lastSuccessfulEnd = i+mn;
				} else {
					// We are not actually completely sure that i+mn is a succeeding index if mn=0
					// So double check it.
					// TODO: This is a lot of code duplication. Tidy this up.
					var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[i+mn]) != turn;
					bool pred = turnPredicate;
					if (turnPredicate) {
					} else {
						trace.Clear();
						if (i+mn == last+1) {
							// Linecast and cost check should in theory always succeed if we are just checking the next node in the path
							// However, in practice, it may pick the "wrong" node in edge cases when going through diagonal connections (could be more costly).
							// So for performance, and robustness, we just assume that the linecast will succeed.
						} else if (gg.Linecast(nodeLast, normalizedLast, nodes[i+mn], normalizedPoints[i+mn], out hit, ref traversalConstraint, trace)) {
							pred = true;
						} else {
							var cost = LinecastCost(trace, points[last], points[i+mn], gg, ref traversalCosts);
							if (cost > costs[i+mn] - costs[last] + COST_FUDGE) {
								// The "shortcut" had such a high penalty that it's not worth taking it
								mxLinecastCost = cost;
								pred = true;
							}
						}
					}

					if (!pred) {
						// Success!
						lastSuccessfulStart = last;
						lastSuccessfulEnd = i+mn;
					}
				}

				// Move to the failing index
				i += mx;
				UnityEngine.Assertions.Assert.AreNotEqual(mxFailMode, PredicateFailMode.Undefined);

				marker5.Begin();
				trace.Clear();
				trace.Clear();
				if (mxFailMode == PredicateFailMode.LinecastCost) {
					outputPath.Add(lastSuccessfulEnd);
					turn = Side2D(points[last], points[lastSuccessfulEnd], points[i]);
					// It is guaranteed that there is line of sight from lastSuccessfulStart to lastSuccessfulEnd
					lastSuccessfulStart = lastSuccessfulEnd;
					i--;
					marker5.End();
					continue;
				} else if (mxFailMode == PredicateFailMode.LinecastObstacle) {
					marker5.End();
					// Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.red);
					marker1.Begin();
					marker3.Begin();
					// Re-run a previously successfully linecast to get all nodes it traversed.
					trace.Clear();
					int chosenCorner;
					if (gg.Linecast(nodes[lastSuccessfulStart], normalizedPoints[lastSuccessfulStart], nodes[lastSuccessfulEnd], normalizedPoints[lastSuccessfulEnd], out hit, ref traversalConstraint, trace)) {
						// Weird! This linecast should have succeeded.
						// Maybe the path crosses some unwalkable nodes it shouldn't cross (the graph could have changed).
						// Or possibly the linecast implementation doesn't handle some edge case (there are so many!)
						// In any case, we fall back to just assuming there is a valid line of sight.
						chosenCorner = lastSuccessfulEnd;
						Debug.LogError("Inconsistent linecasts");
					} else {
						trace.Add(nodes[i]);
						marker3.End();
						marker4.Begin();

						GridNodeBase candidateNode = null;
						var candidateNormalizedPoint = new int2();
						uint candidateCost = 0;
						var dirToCandidateCorner = new int2();
						var lastSuccessfulStartPoint = points[lastSuccessfulStart];
						var lastSuccessfulEndPoint = points[lastSuccessfulEnd];
						var dir = lastSuccessfulEndPoint - lastSuccessfulStartPoint;
						var bounds = new TriangleBounds(
							lastSuccessfulStartPoint,
							lastSuccessfulEndPoint,
							points[i]
							);

						var desiredSide = System.Math.Sign(Cross(dir, points[i] - lastSuccessfulStartPoint));
						var candidateCostSoFar = costs[lastSuccessfulStart];
						for (int j = 0; j < trace.Count; j++) {
							var node = trace[j] as GridNodeBase;
							var nodeGridPos = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid);
							var nodeCenter = nodeGridPos * FixedPrecisionScale;
							// Not perfectly accurate as it doesn't measure the cost to the exact corner
							if (j > 0) candidateCostSoFar += traversalCosts.GetConnectionCost(trace[j-1], trace[j]);
							candidateCostSoFar += (uint)((float)traversalCosts.GetTraversalCostMultiplier(node) * gg.nodeSize * Int3.Precision * IntersectionLength(nodeCenter, lastSuccessfulStartPoint, lastSuccessfulEndPoint));
							for (int d = 0; d < 4; d++) {
								if (!node.HasConnectionInDirection(d) || !traversalConstraint.CanTraverse(node, node.GetNeighbourAlongDirection(d))) {
									for (int q = 0; q < 2; q++) {
										var ncorner = directionToCorners[(d+q)&0x3];
										var corner = nodeCenter + ncorner;

										if (!bounds.Contains(corner)) {
											continue;
										}

										var dirToCorner = corner - lastSuccessfulStartPoint;
										// We shouldn't pick corners at our current position
										if (math.all(dirToCorner == 0)) continue;
										if (math.all(corner == lastSuccessfulEndPoint)) continue;

										var side = Cross(dirToCorner, dirToCandidateCorner);
										if (candidateNode == null || System.Math.Sign(side) == desiredSide || (side == 0 && math.lengthsq(dirToCorner) > math.lengthsq(dirToCandidateCorner))) {
											dirToCandidateCorner = dirToCorner;
											candidateNode = node;
											candidateNormalizedPoint = ncorner;
											candidateCost = candidateCostSoFar;
										}
									}
								}
							}
						}
						marker4.End();

						if (candidateNode == null) {
							// Fall back to adding the lastSuccessfulEnd node. We know there's line of sight to that one.
							chosenCorner = lastSuccessfulEnd;
						} else {
							chosenCorner = numPoints;
							// TODO: Reallocate
							nodes[numPoints] = candidateNode;
							normalizedPoints[numPoints] = candidateNormalizedPoint;
							var gridCoordinates = new int2(candidateNode.XCoordinateInGrid, candidateNode.ZCoordinateInGrid);
							points[numPoints] = gridCoordinates * FixedPrecisionScale + candidateNormalizedPoint;
							costs[numPoints] = candidateCost;
							numPoints++;
						}
					}

					outputPath.Add(chosenCorner);
					turn = Side2D(points[last], points[chosenCorner], points[i]);
					// It is guaranteed that there is line of sight from lastSuccessfulStart to chosenCorner because of how we choose the corner.
					lastSuccessfulStart = chosenCorner;
					i--;
					marker1.End();
					continue;
				} else {
					marker5.End();
					marker2.Begin();
					lastSuccessfulStart = last;
					lastSuccessfulEnd = i;
					// Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.green);
					if (outputPath.Count > 1) {
						var spPrev = outputPath[outputPath.Count-2];
						var nextTurn = Side2D(points[spPrev], points[last], points[i]);
						// Check if the string is no longer taut. If it is not we can remove a previous point.
						if (turn != nextTurn) {
							// Draw.SphereOutline(nodes[pts[pts.Count-1]].UnNormalizePoint(FromFixedPrecision(normalizedPoints[pts[pts.Count-1]])), 0.05f, Color.black);

							lastSuccessfulStart = outputPath[outputPath.Count-2];
							lastSuccessfulEnd = outputPath[outputPath.Count-1];

							outputPath.RemoveAt(outputPath.Count-1);
							if (outputPath.Count > 1) {
								last = spPrev;
								spPrev = outputPath[outputPath.Count-2];
								turn = Side2D(points[spPrev], points[last], points[i]);
							} else {
								// TODO: Should be separate value
								turn = Side.Colinear;
							}
							i--;
							marker2.End();
							continue;
						}
					}
					marker2.End();
				}
			}

			Profiler.EndSample();

			var result = ListPool<Vector3>.Claim(outputPath.Count);
			for (int j = 0; j < outputPath.Count; j++) {
				var idx = outputPath[j];
				result.Add(nodes[idx].UnNormalizePoint(FromFixedPrecision(normalizedPoints[idx])));
			}

			ArrayPool<GridNodeBase>.Release(ref nodes);
			ArrayPool<int2>.Release(ref points);
			ArrayPool<int2>.Release(ref normalizedPoints);
			ArrayPool<uint>.Release(ref costs);
			ListPool<int>.Release(ref outputPath);
			ListPool<GraphNode>.Release(ref trace);
			return result;
		}
	}
}
