using UnityEngine;
using System.Collections.Generic;

namespace Pathfinding.RVO {
	using Pathfinding;
	using Pathfinding.Util;
	using Unity.Burst;
	using Unity.Jobs;
	using Unity.Mathematics;
	using Unity.Collections;
	using Unity.IL2CPP.CompilerServices;
	using Pathfinding.Drawing;
	using Pathfinding.ECS.RVO;
	using static Unity.Burst.CompilerServices.Aliasing;
	using Unity.Profiling;
	using System.Diagnostics;

	[BurstCompile(CompileSynchronously = false, FloatMode = FloatMode.Fast)]
	public struct JobRVOPreprocess : IJob {
		[ReadOnly]
		public SimulatorBurst.AgentData agentData;

		[ReadOnly]
		public SimulatorBurst.AgentOutputData previousOutput;

		[WriteOnly]
		public SimulatorBurst.TemporaryAgentData temporaryAgentData;

		public int startIndex;
		public int endIndex;

		public void Execute () {
			for (int i = startIndex; i < endIndex; i++) {
				if (!agentData.version[i].Valid) continue;

				// Manually controlled overrides the agent being locked.
				// If one for some reason uses them at the same time.
				var locked = agentData.locked[i] & !agentData.manuallyControlled[i];

				if (locked) {
					temporaryAgentData.desiredTargetPointInVelocitySpace[i] = float2.zero;
					temporaryAgentData.desiredVelocity[i] = float3.zero;
					temporaryAgentData.currentVelocity[i] = float3.zero;
				} else {
					var desiredTargetPointInVelocitySpace = agentData.movementPlane[i].ToPlane(agentData.targetPoint[i] - agentData.position[i]);
					temporaryAgentData.desiredTargetPointInVelocitySpace[i] = desiredTargetPointInVelocitySpace;

					// Estimate our current velocity
					// This is necessary because other agents need to know
					// how this agent is moving to be able to avoid it
					var currentVelocity = math.normalizesafe(previousOutput.targetPoint[i] - agentData.position[i]) * previousOutput.speed[i];

					// Calculate the desired velocity from the point we want to reach
					temporaryAgentData.desiredVelocity[i] = agentData.movementPlane[i].ToWorld(math.normalizesafe(desiredTargetPointInVelocitySpace) * agentData.desiredSpeed[i], 0);

					var collisionNormal = math.normalizesafe(agentData.collisionNormal[i]);
					// Check if the velocity is going into the wall
					// If so: remove that component from the velocity
					// Note: if the collisionNormal is zero then the dot prodct will produce a zero as well and nothing will happen.
					float dot = math.dot(currentVelocity, collisionNormal);
					currentVelocity -= math.min(0, dot) * collisionNormal;
					temporaryAgentData.currentVelocity[i] = currentVelocity;
				}
			}
		}
	}

	/// <summary>
	/// Inspired by StarCraft 2's avoidance of locked units.
	/// See: http://www.gdcvault.com/play/1014514/AI-Navigation-It-s-Not
	/// </summary>
	[BurstCompile(FloatMode = FloatMode.Fast)]
	public struct JobHorizonAvoidancePhase1 : Pathfinding.Jobs.IJobParallelForBatched {
		[ReadOnly]
		public SimulatorBurst.AgentData agentData;

		[ReadOnly]
		public NativeArray<float2> desiredTargetPointInVelocitySpace;

		[ReadOnly]
		public NativeArray<int> neighbours;

		public SimulatorBurst.HorizonAgentData horizonAgentData;

		public CommandBuilder draw;

		public bool allowBoundsChecks { get { return true; } }

		/// <summary>
		/// Super simple bubble sort.
		/// TODO: This will be replaced by a better implementation from the Unity.Collections library when that is stable.
		/// </summary>
		static void Sort<T>(NativeSlice<T> arr, NativeSlice<float> keys) where T : struct {
			bool changed = true;

			while (changed) {
				changed = false;
				for (int i = 0; i < arr.Length - 1; i++) {
					if (keys[i] > keys[i+1]) {
						var tmp = keys[i];
						var tmp2 = arr[i];
						keys[i] = keys[i+1];
						keys[i+1] = tmp;
						arr[i] = arr[i+1];
						arr[i+1] = tmp2;
						changed = true;
					}
				}
			}
		}


		/// <summary>Calculates the shortest difference between two given angles given in radians.</summary>
		public static float DeltaAngle (float current, float target) {
			float num = Mathf.Repeat(target - current, math.PI*2);

			if (num > math.PI) {
				num -= math.PI*2;
			}
			return num;
		}

		public void Execute (int startIndex, int count) {
			NativeArray<float> angles = new NativeArray<float>(SimulatorBurst.MaxNeighbourCount*2, Allocator.Temp);
			NativeArray<int> deltas = new NativeArray<int>(SimulatorBurst.MaxNeighbourCount*2, Allocator.Temp);

			for (int i = startIndex; i < startIndex + count; i++) {
				if (!agentData.version[i].Valid) continue;

				if (agentData.locked[i] || agentData.manuallyControlled[i]) {
					horizonAgentData.horizonSide[i] = 0;
					horizonAgentData.horizonMinAngle[i] = 0;
					horizonAgentData.horizonMaxAngle[i] = 0;
					continue;
				}

				float minAngle = 0;
				float maxAngle = 0;

				float desiredAngle = math.atan2(desiredTargetPointInVelocitySpace[i].y, desiredTargetPointInVelocitySpace[i].x);

				int eventCount = 0;

				int inside = 0;

				float radius = agentData.radius[i];

				var position = agentData.position[i];
				var movementPlane = agentData.movementPlane[i];

				var agentNeighbours = neighbours.Slice(i*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);
				for (int j = 0; j < agentNeighbours.Length && agentNeighbours[j] != -1; j++) {
					var other = agentNeighbours[j];
					if (!agentData.locked[other] && !agentData.manuallyControlled[other]) continue;

					var relativePosition = movementPlane.ToPlane(agentData.position[other] - position);
					float dist = math.length(relativePosition);

					float angle = math.atan2(relativePosition.y, relativePosition.x) - desiredAngle;
					float deltaAngle;

					var otherRadius = agentData.radius[other];
					if (dist < radius + otherRadius) {
						// Collision
						deltaAngle = math.PI * 0.49f;
					} else {
						// One degree
						const float AngleMargin = math.PI / 180f;
						deltaAngle = math.asin((radius + otherRadius)/dist) + AngleMargin;
					}

					float aMin = DeltaAngle(0, angle - deltaAngle);
					float aMax = aMin + DeltaAngle(aMin, angle + deltaAngle);

					if (aMin < 0 && aMax > 0) inside++;

					angles[eventCount] = aMin;
					deltas[eventCount] = 1;
					eventCount++;
					angles[eventCount] = aMax;
					deltas[eventCount] = -1;
					eventCount++;
				}

				// If no angle range includes angle 0 then we are already done
				if (inside == 0) {
					horizonAgentData.horizonSide[i] = 0;
					horizonAgentData.horizonMinAngle[i] = 0;
					horizonAgentData.horizonMaxAngle[i] = 0;
					continue;
				}

				// Sort the events by their angle in ascending order
				Sort(deltas.Slice(0, eventCount), angles.Slice(0, eventCount));

				// Find the first index for which the angle is positive
				int firstPositiveIndex = 0;
				for (; firstPositiveIndex < eventCount; firstPositiveIndex++) if (angles[firstPositiveIndex] > 0) break;

				// Walk in the positive direction from angle 0 until the end of the group of angle ranges that include angle 0
				int tmpInside = inside;
				int tmpIndex = firstPositiveIndex;
				for (; tmpIndex < eventCount; tmpIndex++) {
					tmpInside += deltas[tmpIndex];
					if (tmpInside == 0) break;
				}
				maxAngle = tmpIndex == eventCount ? math.PI : angles[tmpIndex];

				// Walk in the negative direction from angle 0 until the end of the group of angle ranges that include angle 0
				tmpInside = inside;
				tmpIndex = firstPositiveIndex - 1;
				for (; tmpIndex >= 0; tmpIndex--) {
					tmpInside -= deltas[tmpIndex];
					if (tmpInside == 0) break;
				}
				minAngle = tmpIndex == -1 ? -math.PI : angles[tmpIndex];

				//horizonBias = -(minAngle + maxAngle);

				// Indicates that a new side should be chosen. The "best" one will be chosen later.
				if (horizonAgentData.horizonSide[i] == 0) horizonAgentData.horizonSide[i] = 2;
				//else horizonBias = math.PI * horizonSide;

				horizonAgentData.horizonMinAngle[i] = minAngle + desiredAngle;
				horizonAgentData.horizonMaxAngle[i] = maxAngle + desiredAngle;
			}
		}
	}

	/// <summary>
	/// Inspired by StarCraft 2's avoidance of locked units.
	/// See: http://www.gdcvault.com/play/1014514/AI-Navigation-It-s-Not
	/// </summary>
	[BurstCompile(FloatMode = FloatMode.Fast)]
	public struct JobHorizonAvoidancePhase2 : Pathfinding.Jobs.IJobParallelForBatched {
		[ReadOnly]
		public NativeArray<int> neighbours;
		[ReadOnly]
		public NativeArray<AgentIndex> versions;
		public NativeArray<float3> desiredVelocity;
		public NativeArray<float2> desiredTargetPointInVelocitySpace;

		[ReadOnly]
		public NativeArray<NativeMovementPlane> movementPlane;

		public SimulatorBurst.HorizonAgentData horizonAgentData;

		public bool allowBoundsChecks => false;

		public void Execute (int startIndex, int count) {
			for (int i = startIndex; i < startIndex + count; i++) {
				if (!versions[i].Valid) continue;

				// Note: Assumes this code is run synchronous (i.e not included in the double buffering part)
				//offsetVelocity = (position - Position) / simulator.DeltaTime;

				if (horizonAgentData.horizonSide[i] == 0) {
					continue;
				}

				if (horizonAgentData.horizonSide[i] == 2) {
					float sum = 0;
					var agentNeighbours = neighbours.Slice(i*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);
					for (int j = 0; j < agentNeighbours.Length && agentNeighbours[j] != -1; j++) {
						var other = agentNeighbours[j];
						var otherHorizonBias = -(horizonAgentData.horizonMinAngle[other] + horizonAgentData.horizonMaxAngle[other]);
						sum += otherHorizonBias;
					}
					var horizonBias = -(horizonAgentData.horizonMinAngle[i] + horizonAgentData.horizonMaxAngle[i]);
					sum += horizonBias;

					horizonAgentData.horizonSide[i] = sum < 0 ? -1 : 1;
				}

				float bestAngle = horizonAgentData.horizonSide[i] < 0 ? horizonAgentData.horizonMinAngle[i] : horizonAgentData.horizonMaxAngle[i];
				float2 desiredDirection;
				math.sincos(bestAngle, out desiredDirection.y, out desiredDirection.x);
				desiredVelocity[i] = movementPlane[i].ToWorld(math.length(desiredVelocity[i]) * desiredDirection, 0);
				desiredTargetPointInVelocitySpace[i] = math.length(desiredTargetPointInVelocitySpace[i]) * desiredDirection;
			}
		}
	}

	[BurstCompile(FloatMode = FloatMode.Fast)]
	public struct JobHardCollisions<MovementPlaneWrapper> : Pathfinding.Jobs.IJobParallelForBatched where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
		[ReadOnly]
		public SimulatorBurst.AgentData agentData;
		[ReadOnly]
		public NativeArray<int> neighbours;
		[WriteOnly]
		public NativeArray<float2> collisionVelocityOffsets;

		public float deltaTime;
		public bool enabled;

		/// <summary>
		/// How aggressively hard collisions are resolved.
		/// Should be a value between 0 and 1.
		/// </summary>
		const float CollisionStrength = 0.8f;

		public bool allowBoundsChecks => false;

		public void Execute (int startIndex, int count) {
			if (!enabled) {
				for (int i = startIndex; i < startIndex + count; i++) {
					collisionVelocityOffsets[i] = float2.zero;
				}
				return;
			}

			for (int i = startIndex; i < startIndex + count; i++) {
				if (!agentData.version[i].Valid || agentData.locked[i]) {
					collisionVelocityOffsets[i] = float2.zero;
					continue;
				}

				var agentNeighbours = neighbours.Slice(i*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);
				var radius = agentData.radius[i];
				var totalOffset = float2.zero;
				float totalWeight = 0;

				var position = agentData.position[i];
				var movementPlane = new MovementPlaneWrapper();
				movementPlane.Set(agentData.movementPlane[i]);

				for (int j = 0; j < agentNeighbours.Length && agentNeighbours[j] != -1; j++) {
					var other = agentNeighbours[j];
					var relativePosition = movementPlane.ToPlane(position - agentData.position[other]);

					var dirSqrLength = math.lengthsq(relativePosition);
					var combinedRadius = agentData.radius[other] + radius;
					if (dirSqrLength < combinedRadius*combinedRadius && dirSqrLength > 0.00000001f) {
						// Collision
						var dirLength = math.sqrt(dirSqrLength);
						var normalizedDir = relativePosition * (1.0f / dirLength);

						// Overlap amount
						var weight = combinedRadius - dirLength;

						// Position offset required to make the agents not collide anymore
						var offset = normalizedDir * weight;
						// In a later step a weighted average will be taken so that the average offset is extracted
						var weightedOffset = offset * weight;

						totalOffset += weightedOffset;
						totalWeight += weight;
					}
				}

				var offsetVelocity = totalOffset * (1.0f / (0.0001f + totalWeight));
				offsetVelocity *= (CollisionStrength * 0.5f) / deltaTime;

				collisionVelocityOffsets[i] = offsetVelocity;
			}
		}
	}

	[BurstCompile(CompileSynchronously = false, FloatMode = FloatMode.Fast)]
	public struct JobRVOCalculateNeighbours<MovementPlaneWrapper> : Pathfinding.Jobs.IJobParallelForBatched where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
		[ReadOnly]
		public SimulatorBurst.AgentData agentData;

		[ReadOnly]
		public RVOQuadtreeBurst quadtree;

		public NativeArray<int> outNeighbours;

		[WriteOnly]
		public SimulatorBurst.AgentOutputData output;

		public bool allowBoundsChecks { get { return false; } }

		public void Execute (int startIndex, int count) {
			NativeArray<float> neighbourDistances = new NativeArray<float>(SimulatorBurst.MaxNeighbourCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);

			for (int i = startIndex; i < startIndex + count; i++) {
				if (!agentData.version[i].Valid) continue;
				CalculateNeighbours(i, outNeighbours, neighbourDistances);
			}
		}

		void CalculateNeighbours (int agentIndex, NativeArray<int> neighbours, NativeArray<float> neighbourDistances) {
			int maxNeighbourCount = math.min(SimulatorBurst.MaxNeighbourCount, agentData.maxNeighbours[agentIndex]);
			// Write the output starting at this index in the neighbours array
			var outputIndex = agentIndex * SimulatorBurst.MaxNeighbourCount;

			quadtree.QueryKNearest(new RVOQuadtreeBurst.QuadtreeQuery {
				position = agentData.position[agentIndex],
				speed = agentData.maxSpeed[agentIndex],
				agentRadius = agentData.radius[agentIndex],
				timeHorizon = agentData.agentTimeHorizon[agentIndex],
				outputStartIndex = outputIndex,
				maxCount = maxNeighbourCount,
				result = neighbours,
				resultDistances = neighbourDistances,
			});

			int numNeighbours = 0;
			while (numNeighbours < maxNeighbourCount && math.isfinite(neighbourDistances[numNeighbours])) numNeighbours++;
			output.numNeighbours[agentIndex] = numNeighbours;

			MovementPlaneWrapper movementPlane = default;
			movementPlane.Set(agentData.movementPlane[agentIndex]);
			movementPlane.ToPlane(agentData.position[agentIndex], out float localElevation);

			// Filter out invalid neighbours
			for (int i = 0; i < numNeighbours; i++) {
				int otherIndex = neighbours[outputIndex + i];
				// Interval along the y axis in which the agents overlap
				movementPlane.ToPlane(agentData.position[otherIndex], out float otherElevation);
				float maxY = math.min(localElevation + agentData.height[agentIndex], otherElevation + agentData.height[otherIndex]);
				float minY = math.max(localElevation, otherElevation);

				// The agents cannot collide if they are on different y-levels.
				// Also do not avoid the agent itself.
				// Apply the layer masks for agents.
				// Use binary OR to reduce branching.
				if ((maxY < minY) | (otherIndex == agentIndex) | (((int)agentData.collidesWith[agentIndex] & (int)agentData.layer[otherIndex]) == 0)) {
					numNeighbours--;
					neighbours[outputIndex + i] = neighbours[outputIndex + numNeighbours];
					i--;
				}
			}

			// Add a token indicating the size of the neighbours list
			if (numNeighbours < SimulatorBurst.MaxNeighbourCount) neighbours[outputIndex + numNeighbours] = -1;
		}
	}

	/// <summary>
	/// Calculates if the agent has reached the end of its path and if its blocked from further progress towards it.
	///
	/// If many agents have the same destination they can often end up crowded around a single point.
	/// It is often desirable to detect this and mark all agents around that destination as having at least
	/// partially reached the end of their paths.
	///
	/// This job uses the following heuristics to determine this:
	///
	/// 1. If an agent wants to move in a particular direction, but there's another agent in the way that makes it have to reduce its velocity,
	///     the other agent is considered to be "blocking" the current agent.
	/// 2. If the agent is within a small distance of the destination
	///        THEN it is considered to have reached the end of its path.
	/// 3. If the agent is blocked by another agent,
	///        AND the other agent is blocked by this agent in turn,
	///        AND if the destination is between the two agents,
	///        THEN the the agent is considered to have reached the end of its path.
	/// 4. If the agent is blocked by another agent which has reached the end of its path,
	///        AND this agent is is moving slowly
	///        AND this agent cannot move furter forward than 50% of its radius.
	///        THEN the agent is considered to have reached the end of its path.
	///
	/// Heuristics 2 and 3 are calculated initially, and then using heuristic 4 the set of agents which have reached their destinations expands outwards.
	///
	/// These heuristics are robust enough that they can be used even if for example the agents are stuck in a winding maze
	/// and only one agent is actually able to reach the destination.
	///
	/// This job doesn't affect the movement of the agents by itself.
	/// However, it is built with the intention that the FlowFollowingStrength parameter will be set
	/// elsewhere to 1 for agents which have reached the end of their paths. This will make the agents stop gracefully
	/// when the end of their paths is crowded instead of continuing to try to desperately reach the destination.
	/// </summary>
	[BurstCompile(CompileSynchronously = false, FloatMode = FloatMode.Fast)]
	public struct JobDestinationReached<MovementPlaneWrapper>: IJob where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
		[ReadOnly]
		public SimulatorBurst.AgentData agentData;

		[ReadOnly]
		public SimulatorBurst.TemporaryAgentData temporaryAgentData;

		[ReadOnly]
		public SimulatorBurst.ObstacleData obstacleData;

		public SimulatorBurst.AgentOutputData output;
		public int numAgents;
		public CommandBuilder draw;

		private static readonly ProfilerMarker MarkerInvert = new ProfilerMarker("InvertArrows");
		private static readonly ProfilerMarker MarkerAlloc = new ProfilerMarker("Alloc");
		private static readonly ProfilerMarker MarkerFirstPass = new ProfilerMarker("FirstPass");

		struct TempAgentData {
			public bool blockedAndSlow;
			public float distToEndSq;
		}

		public void Execute () {
			MarkerAlloc.Begin();
			for (int agentIndex = 0; agentIndex < numAgents; agentIndex++) {
				output.effectivelyReachedDestination[agentIndex] = ReachedEndOfPath.NotReached;
			}

			// For each agent, store which agents it blocks
			var inArrows = new NativeArray<int>(agentData.position.Length*SimulatorBurst.MaxBlockingAgentCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			// Number of agents that each agent blocks
			var inArrowCounts = new NativeArray<int>(agentData.position.Length, Allocator.Temp, NativeArrayOptions.ClearMemory);
			var que = new NativeCircularBuffer<int>(16, Allocator.Temp);
			// True for an agent if it is in the queue, or if it should never be queued again
			var queued = new NativeArray<bool>(numAgents, Allocator.Temp, NativeArrayOptions.ClearMemory);
			var tempData = new NativeArray<TempAgentData>(numAgents, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			MarkerAlloc.End();
			MarkerInvert.Begin();

			for (int agentIndex = 0; agentIndex < numAgents; agentIndex++) {
				if (!agentData.version[agentIndex].Valid) continue;
				for (int i = 0; i < SimulatorBurst.MaxBlockingAgentCount; i++) {
					var blockingAgentIndex = output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
					if (blockingAgentIndex == -1) break;
					var count = inArrowCounts[blockingAgentIndex];
					if (count >= SimulatorBurst.MaxBlockingAgentCount) continue;
					inArrows[blockingAgentIndex*SimulatorBurst.MaxBlockingAgentCount + count] = agentIndex;
					inArrowCounts[blockingAgentIndex] = count+1;
				}
			}
			MarkerInvert.End();

			MarkerFirstPass.Begin();
			for (int agentIndex = 0; agentIndex < numAgents; agentIndex++) {
				if (!agentData.version[agentIndex].Valid) continue;

				var position = agentData.position[agentIndex];
				var movementPlane = agentData.movementPlane[agentIndex];
				var ourSpeed = output.speed[agentIndex];
				var ourEndOfPath = agentData.endOfPath[agentIndex];

				// Ignore if destination is not set
				if (!math.isfinite(ourEndOfPath.x)) continue;

				var distToEndSq = math.lengthsq(movementPlane.ToPlane(ourEndOfPath - position, out float endOfPathElevationDifference));
				var ourHeight = agentData.height[agentIndex];
				var reachedEndOfPath = false;
				var flowFollowing = false;
				var ourRadius = agentData.radius[agentIndex];
				var forwardClearance = output.forwardClearance[agentIndex];

				// Heuristic 2
				if (distToEndSq < ourRadius*ourRadius*(0.5f*0.5f) && endOfPathElevationDifference < ourHeight && endOfPathElevationDifference > -ourHeight*0.5f) {
					reachedEndOfPath = true;
				}

				var closeToBlocked = forwardClearance < ourRadius*0.5f;
				var slowish = ourSpeed*ourSpeed < math.max(0.01f*0.01f, math.lengthsq(temporaryAgentData.desiredVelocity[agentIndex])*0.25f);
				var blockedAndSlow = closeToBlocked && slowish;
				tempData[agentIndex] = new TempAgentData {
					blockedAndSlow = blockedAndSlow,
					distToEndSq = distToEndSq
				};

				// Heuristic 3
				for (int i = 0; i < SimulatorBurst.MaxBlockingAgentCount; i++) {
					var blockingAgentIndex = output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
					if (blockingAgentIndex == -1) break;

					var otherPosition = agentData.position[blockingAgentIndex];
					var distBetweenAgentsSq = math.lengthsq(movementPlane.ToPlane(position - otherPosition));
					var circleRadius = (math.sqrt(distBetweenAgentsSq) + ourRadius + agentData.radius[blockingAgentIndex])*0.5f;
					var endWithinCircle = math.lengthsq(movementPlane.ToPlane(ourEndOfPath - 0.5f*(position + otherPosition))) < circleRadius*circleRadius;
					if (endWithinCircle) {
						// Check if the other agent has an arrow pointing to this agent (i.e. it is blocked by this agent)
						var loop = false;
						for (int j = 0; j < SimulatorBurst.MaxBlockingAgentCount; j++) {
							var arrowFromAgent = inArrows[agentIndex*SimulatorBurst.MaxBlockingAgentCount + j];
							if (arrowFromAgent == -1) break;
							if (arrowFromAgent == blockingAgentIndex) {
								loop = true;
								break;
							}
						}

						if (loop) {
							flowFollowing = true;

							if (blockedAndSlow) {
								reachedEndOfPath = true;
							}
						}
					}
				}

				var effectivelyReached = reachedEndOfPath ? ReachedEndOfPath.Reached : (flowFollowing ? ReachedEndOfPath.ReachedSoon : ReachedEndOfPath.NotReached);
				if (effectivelyReached != output.effectivelyReachedDestination[agentIndex]) {
					output.effectivelyReachedDestination[agentIndex] = effectivelyReached;

					if (effectivelyReached == ReachedEndOfPath.Reached) {
						// Mark this agent as queued to prevent it from being added to the queue again.
						queued[agentIndex] = true;

						// Changing to the Reached flag may affect the calculations for other agents.
						// So we iterate over all agents that may be affected and enqueue them again.
						var count = inArrowCounts[agentIndex];
						for (int i = 0; i < count; i++) {
							var inArrow = inArrows[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
							if (!queued[inArrow]) que.PushEnd(inArrow);
						}
					}
				}
			}
			MarkerFirstPass.End();


			int iteration = 0;
			while (que.Length > 0) {
				var agentIndex = que.PopStart();
				iteration++;
				// If we are already at the reached stage, the result can never change.
				if (output.effectivelyReachedDestination[agentIndex] == ReachedEndOfPath.Reached) continue;
				queued[agentIndex] = false;

				var ourSpeed = output.speed[agentIndex];
				var ourEndOfPath = agentData.endOfPath[agentIndex];
				// Ignore if destination is not set
				if (!math.isfinite(ourEndOfPath.x)) continue;

				var ourPosition = agentData.position[agentIndex];
				var blockedAndSlow = tempData[agentIndex].blockedAndSlow;
				var distToEndSq = tempData[agentIndex].distToEndSq;
				var ourRadius = agentData.radius[agentIndex];
				var reachedEndOfPath = false;
				var flowFollowing = false;

				// Heuristic 4
				for (int i = 0; i < SimulatorBurst.MaxBlockingAgentCount; i++) {
					var blockingAgentIndex = output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
					if (blockingAgentIndex == -1) break;

					var otherEndOfPath = agentData.endOfPath[blockingAgentIndex];
					var otherRadius = agentData.radius[blockingAgentIndex];

					// Check if the other agent has a destination in roughly the same position as this agent.
					// If we are further from the destination we tolarate larger deviations.
					var endOfPathsOverlapping = math.lengthsq(otherEndOfPath - ourEndOfPath) <= distToEndSq*(0.5f*0.5f);
					var otherReached = output.effectivelyReachedDestination[blockingAgentIndex] == ReachedEndOfPath.Reached;

					if (otherReached && (endOfPathsOverlapping || math.lengthsq(ourEndOfPath - agentData.position[blockingAgentIndex]) < math.lengthsq(ourRadius+otherRadius))) {
						var otherSpeed = output.speed[blockingAgentIndex];
						flowFollowing |= math.min(ourSpeed, otherSpeed) < 0.01f;
						reachedEndOfPath |= blockedAndSlow;
					}
				}

				var effectivelyReached = reachedEndOfPath ? ReachedEndOfPath.Reached : (flowFollowing ? ReachedEndOfPath.ReachedSoon : ReachedEndOfPath.NotReached);
				// We do not check for all things that are checked in the first pass. So incorporate the previous information by taking the max.
				effectivelyReached = (ReachedEndOfPath)math.max((int)effectivelyReached, (int)output.effectivelyReachedDestination[agentIndex]);

				if (effectivelyReached != output.effectivelyReachedDestination[agentIndex]) {
					output.effectivelyReachedDestination[agentIndex] = effectivelyReached;

					if (effectivelyReached == ReachedEndOfPath.Reached) {
						// Mark this agent as queued to prevent it from being added to the queue again.
						queued[agentIndex] = true;

						// Changes to the Reached flag may affect the calculations for other agents.
						// So we iterate over all agents that may be affected and enqueue them again.
						var count = inArrowCounts[agentIndex];
						for (int i = 0; i < count; i++) {
							var inArrow = inArrows[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
							if (!queued[inArrow]) que.PushEnd(inArrow);
						}
					}
				}
			}
		}
	}

	// Note: FloatMode should not be set to Fast because that causes inaccuracies which can lead to
	// agents failing to avoid walls sometimes.
	[BurstCompile(CompileSynchronously = true, FloatMode = FloatMode.Default)]
	public struct JobRVO<MovementPlaneWrapper> : Pathfinding.Jobs.IJobParallelForBatched where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
		[ReadOnly]
		public SimulatorBurst.AgentData agentData;

		[ReadOnly]
		public SimulatorBurst.TemporaryAgentData temporaryAgentData;

		[ReadOnly]
		public NavmeshEdges.NavmeshBorderData navmeshEdgeData;

		[WriteOnly]
		public SimulatorBurst.AgentOutputData output;

		public float deltaTime;
		public float symmetryBreakingBias;
		public float priorityMultiplier;
		public bool useNavmeshAsObstacle;

		public bool allowBoundsChecks { get { return true; } }

		const int MaxObstacleCount = 50;

		public CommandBuilder draw;

		public void Execute (int startIndex, int batchSize) {
			ExecuteORCA(startIndex, batchSize);
		}

		struct SortByKey : IComparer<int> {
			public UnsafeSpan<float> keys;

			public int Compare (int x, int y) {
				return keys[x].CompareTo(keys[y]);
			}
		}

		/// <summary>
		/// Sorts the array in place using insertion sort.
		/// This is a stable sort.
		/// See: http://en.wikipedia.org/wiki/Insertion_sort
		///
		/// Used only because Unity.Collections.NativeSortExtension.Sort seems to have some kind of code generation bug when using Burst 1.8.2, causing it to throw exceptions.
		/// </summary>
		static void InsertionSort<T, U>(UnsafeSpan<T> data, U comparer) where T : unmanaged where U : IComparer<T> {
			for (int i = 1; i < data.Length; i++) {
				var value = data[i];
				int j = i - 1;
				while (j >= 0 && comparer.Compare(data[j], value) > 0) {
					data[j + 1] = data[j];
					j--;
				}
				data[j + 1] = value;
			}
		}

		private static readonly ProfilerMarker MarkerConvertObstacles1 = new ProfilerMarker("RVOConvertObstacles1");
		private static readonly ProfilerMarker MarkerConvertObstacles2 = new ProfilerMarker("RVOConvertObstacles2");

		/// <summary>
		/// Generates ORCA half-planes for all obstacles near the agent.
		/// For more details refer to the ORCA (Optimal Reciprocal Collision Avoidance) paper.
		///
		/// This function takes in several arrays which are just used for temporary data. This is to avoid the overhead of allocating the arrays once for every agent.
		/// </summary>
		void GenerateObstacleVOs (int agentIndex, NativeList<int> adjacentObstacleIdsScratch, NativeArray<int2> adjacentObstacleVerticesScratch, NativeArray<float> segmentDistancesScratch, NativeArray<int> sortedVerticesScratch, NativeArray<ORCALine> orcaLines, NativeArray<int> orcaLineToAgent, [NoAlias] ref int numLines, [NoAlias] in MovementPlaneWrapper movementPlane, float2 optimalVelocity) {
			if (!useNavmeshAsObstacle) return;

			var localPosition = movementPlane.ToPlane(agentData.position[agentIndex], out var agentElevation);
			var agentHeight = agentData.height[agentIndex];
			var agentRadius = agentData.radius[agentIndex];
			var obstacleRadius = agentRadius * 0.01f;
			var inverseObstacleTimeHorizon = math.rcp(agentData.obstacleTimeHorizon[agentIndex]);

			ExpectNotAliased(in agentData.collisionNormal, in agentData.position);

			var hierarchicalNodeIndex = agentData.hierarchicalNodeIndex[agentIndex];
			if (hierarchicalNodeIndex == -1) return;

			var size = (obstacleRadius + agentRadius + agentData.obstacleTimeHorizon[agentIndex] * agentData.maxSpeed[agentIndex]) * new float3(2, 0, 2);
			size.y = agentData.height[agentIndex] * 2f;
			var bounds = new Bounds(new Vector3(localPosition.x, agentElevation, localPosition.y), size);
			var boundingRadiusSq = math.lengthsq(bounds.extents);
			adjacentObstacleIdsScratch.Clear();

			var worldBounds = movementPlane.ToWorld(bounds);
			navmeshEdgeData.GetObstaclesInRange(hierarchicalNodeIndex, worldBounds, adjacentObstacleIdsScratch);

#if UNITY_EDITOR
			if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.Obstacles)) {
				draw.PushMatrix(movementPlane.matrix);
				draw.PushMatrix(new float4x4(
					new float4(1, 0, 0, 0),
					new float4(0, 0, -1, 0),
					new float4(0, 1, 0, 0),
					new float4(0, 0, 0, 1)
					));
				draw.WireBox(bounds, Color.blue);
				draw.PopMatrix();
				draw.PopMatrix();
			}
#endif

			// TODO: For correctness all obstacles should be added in nearest-to-farthest order.
			// This loop should be split up.
			for (int oi = 0; oi < adjacentObstacleIdsScratch.Length; oi++) {
				MarkerConvertObstacles1.Begin();
				var obstacleId = adjacentObstacleIdsScratch[oi];

				var obstacleAllocations = navmeshEdgeData.obstacleData.obstacles[obstacleId];
				var vertices = navmeshEdgeData.obstacleData.obstacleVertices.GetSpan(obstacleAllocations.verticesAllocation);
				var groups = navmeshEdgeData.obstacleData.obstacleVertexGroups.GetSpan(obstacleAllocations.groupsAllocation);
				int vertexOffset = 0;
				int candidateVertexCount = 0;
				for (int i = 0; i < groups.Length; i++) {
					var group = groups[i];
					// Check if the group does not overlap with our bounds at all
					if (!math.all((group.boundsMx >= worldBounds.min) & (group.boundsMn <= worldBounds.max))) {
						vertexOffset += group.vertexCount;
						continue;
					}


					var startVertex = vertexOffset;
					var endVertex = vertexOffset + group.vertexCount - 1;
					if (endVertex >= adjacentObstacleVerticesScratch.Length) {
						// Too many vertices. Skip remaining vertices.
						break;
					}

					for (int vi = startVertex; vi < startVertex + group.vertexCount; vi++) {
						// X coordinate is the index of the previous vertex, the y coordinate is the next vertex
						adjacentObstacleVerticesScratch[vi] = new int2(vi - 1, vi + 1);
					}
					// UnityEngine.Assertions.Assert.AreEqual(vertexCount, endVertex + 1);

					// Patch the start and end vertices to be correct.
					// In a chain the last vertex doesn't start a new segment so we just make it loop back on itself.
					// In a loop the last vertex connects to the first vertex.
					adjacentObstacleVerticesScratch[startVertex] = new int2(group.type == ObstacleType.Loop ? endVertex : startVertex, adjacentObstacleVerticesScratch[startVertex].y);
					adjacentObstacleVerticesScratch[endVertex] = new int2(adjacentObstacleVerticesScratch[endVertex].x, group.type == ObstacleType.Loop ? startVertex : endVertex);

					for (int vi = 0; vi < group.vertexCount; vi++) {
						var vertex = vertices[vi + vertexOffset];
						int next = adjacentObstacleVerticesScratch[vi + startVertex].y;
						var pos = movementPlane.ToPlane(vertex) - localPosition;
						var nextPos = movementPlane.ToPlane(vertices[next]) - localPosition;
						var dir = nextPos - pos;
						var closestT = ClosestPointOnSegment(pos, dir / math.lengthsq(dir), float2.zero, 0, 1);
						var dist = math.lengthsq(pos + dir*closestT);
						segmentDistancesScratch[vi + startVertex] = dist;

						if (dist <= boundingRadiusSq && candidateVertexCount < sortedVerticesScratch.Length) {
							sortedVerticesScratch[candidateVertexCount] = vi + startVertex;
							candidateVertexCount++;
						}
					}

					vertexOffset += group.vertexCount;
				}

				MarkerConvertObstacles1.End();

				MarkerConvertObstacles2.Begin();
				// Sort obstacle segments by distance from the agent
				InsertionSort(sortedVerticesScratch.AsUnsafeSpan().Slice(0, candidateVertexCount), new SortByKey {
					keys = segmentDistancesScratch.AsUnsafeSpan().Slice(0, vertexOffset)
				});

				for (int i = 0; i < candidateVertexCount; i++) {
					// In the unlikely event that we exceed the maximum number of obstacles, we just skip the remaining ones.
					if (numLines >= MaxObstacleCount) break;

					// Processing the obstacle defined by v1 and v2
					//
					// v0                v3
					//   \               /
					//    \             /
					//    v1 ========= v2
					//
					var v1Index = sortedVerticesScratch[i];

					// If the obstacle is too far away, we can skip it.
					// Since the obstacles are sorted by distance we can break here.
					if (segmentDistancesScratch[v1Index] > 0.25f*size.x*size.x) break;

					var v0Index = adjacentObstacleVerticesScratch[v1Index].x;
					var v2Index = adjacentObstacleVerticesScratch[v1Index].y;
					if (v2Index == v1Index) continue;
					var v3Index = adjacentObstacleVerticesScratch[v2Index].y;
					UnityEngine.Assertions.Assert.AreNotEqual(v1Index, v3Index);
					UnityEngine.Assertions.Assert.AreNotEqual(v0Index, v2Index);

					var v0 = vertices[v0Index];
					var v1 = vertices[v1Index];
					var v2 = vertices[v2Index];
					var v3 = vertices[v3Index];

					var v0Position = movementPlane.ToPlane(v0) - localPosition;
					var v1Position = movementPlane.ToPlane(v1, out var e1) - localPosition;
					var v2Position = movementPlane.ToPlane(v2, out var e2) - localPosition;
					var v3Position = movementPlane.ToPlane(v3) - localPosition;

					// Assume the obstacle has the same height as the agent, then check if they overlap along the elevation axis.
					if (math.max(e1, e2) + agentHeight < agentElevation || math.min(e1, e2) > agentElevation + agentHeight) {
						// The obstacle is not in the agent's elevation range. Ignore it.
						continue;
					}

					var length = math.length(v2Position - v1Position);
					if (length < 0.0001f) continue;
					var segmentDir = (v2Position - v1Position) * math.rcp(length);

					if (det(segmentDir, -v1Position) > obstacleRadius) {
						// Agent is significantly on the wrong side of the segment (on the "inside"). Ignore it.
						continue;
					}

					// Check if this velocity obstacle completely behind previously added ORCA lines.
					// If so, this obstacle is redundant and we can ignore it.
					// This is not just a performance optimization. Using the ORCA lines for closer
					// obstacles is better since obstacles further away can add ORCA lines that
					// restrict the velocity space unnecessarily. The ORCA line is more conservative than the VO.
					bool alreadyCovered = false;

					const float EPSILON = 0.0001f;
					for (var j = 0; j < numLines; j++) {
						var line = orcaLines[j];
						if (
							// Check if this velocity-obstacle is completely inside the previous ORCA line's infeasible half-plane region.
							det(inverseObstacleTimeHorizon * v1Position - line.point, line.direction) - inverseObstacleTimeHorizon * obstacleRadius >= -EPSILON &&
							det(inverseObstacleTimeHorizon * v2Position - line.point, line.direction) - inverseObstacleTimeHorizon * obstacleRadius >= -EPSILON
							) {
							alreadyCovered = true;
							break;
						}
					}
					if (alreadyCovered) {
						continue;
					}

					var obstacleOptimizationVelocity = float2.zero;
					var distanceAlongSegment = math.dot(obstacleOptimizationVelocity - v1Position, segmentDir);
					var closestPointOnSegment = v1Position + distanceAlongSegment * segmentDir;
					var distanceToLineSq = math.lengthsq(closestPointOnSegment - obstacleOptimizationVelocity);
					var distanceToSegmentSq = math.lengthsq((v1Position + math.clamp(distanceAlongSegment, 0, length) * segmentDir));

					var v1Convex = leftOrColinear(v1Position - v0Position, segmentDir);
					var v2Convex = leftOrColinear(segmentDir, v3Position - v2Position);

					if (distanceToSegmentSq < obstacleRadius*obstacleRadius) {
						if (distanceAlongSegment < 0.0f) {
							// Collision with left vertex, ignore if the vertex is not convex
							if (v1Convex) {
								orcaLineToAgent[numLines] = -1;
								orcaLines[numLines++] = new ORCALine {
									point = -v1Position * 0.1f,
									direction = math.normalizesafe(rot90(v1Position)),
								};
							}
						} else if (distanceAlongSegment > length) {
							// Collision with right vertex
							// Ignore if the vertex is not convex, or if it will be taken care of
							// by the neighbour obstacle segment.
							if (v2Convex && leftOrColinear(v2Position, v3Position - v2Position)) {
								orcaLineToAgent[numLines] = -1;
								orcaLines[numLines++] = new ORCALine {
									point = -v2Position * 0.1f,
									direction = math.normalizesafe(rot90(v2Position)),
								};
							}
						} else {
							// Collision with segment
							orcaLineToAgent[numLines] = -1;
							orcaLines[numLines++] = new ORCALine {
								point = -closestPointOnSegment * 0.1f,
								direction = -segmentDir,
							};
						}
						continue;
					}

					// Represents rays starting points on the VO circles, going in a tangent direction away from the agent.
					float2 leftLegDirection, rightLegDirection;

					if ((distanceAlongSegment < 0 || distanceAlongSegment > 1) && distanceToLineSq <= obstacleRadius*obstacleRadius) {
						// Obliquely viewed so that the circle around one of the vertices is all that is visible from p. p = obstacleOptimizationVelocity
						//     _____________________________  _ _ _ _ _ _ _ _ _ _ _ _
						//   _/     \_               _/     \_
						//  /         \             /         \
						//  |    v1   |             |    v2   |
						//  \_       _/             \_       _/          p
						//    \_____/_________________\_____/ _ _ _ _ _ _ _ _ _ _ _ _

						// Collapse segment to a single point, making sure that v0 and v3 are still the neighbouring vertices.
						if (distanceAlongSegment < 0) {
							// Collapse to v1
							// Ignore if not convex
							if (!v1Convex) continue;
							v3Position = v2Position;
							v2Position = v1Position;
							v2Convex = v1Convex;
						} else {
							// Collapse to v2
							if (!v2Convex) continue;
							v0Position = v1Position;
							v1Position = v2Position;
							v1Convex = v2Convex;
						}
						var vertexDistSq = math.lengthsq(v1Position);
						// Distance from p to the points where the legs (tangents) touch the circle around the vertex.
						float leg = math.sqrt(vertexDistSq - obstacleRadius*obstacleRadius);
						var posNormal = new float2(-v1Position.y, v1Position.x);
						// These become normalized
						leftLegDirection = (v1Position*leg + posNormal*obstacleRadius) / vertexDistSq;
						rightLegDirection = (v1Position*leg - posNormal*obstacleRadius) / vertexDistSq;
					} else {
						// This is the common case (several valid positions of p are shown). p = obstacleOptimizationVelocity
						//
						//        p
						//     _____________________________
						//   _/     \_               _/     \_
						//  /         \             /         \
						//  |    v1   |             |    v2   |
						//  \_       _/             \_       _/
						//    \_____/_________________\_____/
						//
						//                       p                   p

						if (v1Convex) {
							var vertexDistSq = math.lengthsq(v1Position);
							float leg = math.sqrt(vertexDistSq - obstacleRadius*obstacleRadius);
							var posNormal = new float2(-v1Position.y, v1Position.x);
							// This becomes normalized
							leftLegDirection = (v1Position*leg + posNormal*obstacleRadius) / vertexDistSq;
						} else {
							leftLegDirection = -segmentDir;
						}

						if (v2Convex) {
							var vertexDistSq = math.lengthsq(v2Position);
							float leg = math.sqrt(vertexDistSq - obstacleRadius*obstacleRadius);
							var posNormal = new float2(-v2Position.y, v2Position.x);
							rightLegDirection = (v2Position*leg - posNormal*obstacleRadius) / vertexDistSq;
						} else {
							rightLegDirection = segmentDir;
						}
					}

					// Legs should never point into the obstacle for legs added by convex vertices.
					// The neighbouring vertex will add a better obstacle for those cases.
					//
					// In that case we replace the legs with the neighbouring segments, and if the closest
					// point is on those segments we know we can ignore them because the
					// neighbour will handle it.
					//
					// It's important that we don't include the case when they are colinear,
					// because if v1=v0 (or v2=v3), which can happen at the end of a chain, the
					// determinant will always be zero and so they will seem colinear.
					//
					// Note: One might think that this should apply to all vertices, not just convex ones.
					// Consider this case where you might think a non-convex vertices otherwise would
					// cause 'ghost' obstacles:
					//   ___
					//  |  | A
					//  |  |
					//  |   \
					//  |____\ B
					//       <-X
					//
					// If X is an agent, moving to the left. It could get stuck against the segment A.
					// This is because the vertex between A and B is concave, and it will generate a leg
					// pointing downwards.
					//
					// However, this does not cause a problem in practice. Because if the horizontal segment at the bottom is added first (as it should be)
					// then A and B will be discarded since they will be completely behind the ORCA line added by the horizontal segment.
					bool isLeftLegForeign = false;
					bool isRightLegForeign = false;
					if (v1Convex && left(leftLegDirection, v0Position - v1Position)) {
						// Left leg points into obstacle
						leftLegDirection = v0Position - v1Position;
						isLeftLegForeign = true;
					}

					if (v2Convex && right(rightLegDirection, v3Position - v2Position)) {
						// Right leg points into obstacle
						rightLegDirection = v3Position - v2Position;
						isRightLegForeign = true;
					}


					// The velocity obstacle for this segment consists of a left leg, right leg,
					// a cutoff line, and two circular arcs where the legs and the cutoff line join together.
					// LeftLeg                                   RightLeg
					//    \      _____________________________      /
					//     \   _/     \_               _/     \_   /
					//      \ /         \             /         \ /
					//       \|    v1   |             |    v2   |/
					//        \_       _/             \_       _/
					//          \_____/_________________\_____/
					//                     Cutoff Line
					//
					// In case only one vertex makes up the obstacle then we instead have just a left leg, right leg, and a single circular arc.
					//
					//  LeftLeg           RightLeg
					//    \      _____      /
					//     \   _/     \_   /
					//      \ /         \ /
					//       \|         |/
					//        \_       _/
					//          \_____/
					//


					// We first check if the velocity will be projected on those circular segments.
					var leftCutoff = inverseObstacleTimeHorizon * v1Position;
					var rightCutoff = inverseObstacleTimeHorizon * v2Position;
					var cutoffDir = rightCutoff - leftCutoff;
					var cutoffLength = math.lengthsq(cutoffDir);

					// Projection on the cutoff line (between 0 and 1 if the projection is on the cutoff segment)
					var t = cutoffLength <= 0.00001f ? 0.5f : math.dot(optimalVelocity - leftCutoff, cutoffDir)/cutoffLength;
					// Negative if the closest point on the rays reprensenting the legs is before the ray starts
					var tLeft = math.dot(optimalVelocity - leftCutoff, leftLegDirection);
					var tRight = math.dot(optimalVelocity - rightCutoff, rightLegDirection);


					// Check if the projected velocity is on the circular arcs
					if ((t < 0.0f && tLeft < 0.0f) || (t > 1.0f && tRight < 0.0f) || (cutoffLength <= 0.00001f && tLeft < 0.0f && tRight < 0.0f)) {
						var arcCenter = t <= 0.5f ? leftCutoff : rightCutoff;

						var unitW = math.normalizesafe(optimalVelocity - arcCenter);
						orcaLineToAgent[numLines] = -1;
						orcaLines[numLines++] = new ORCALine {
							point = arcCenter + obstacleRadius * inverseObstacleTimeHorizon * unitW,
							direction = new float2(unitW.y, -unitW.x),
						};
						continue;
					}

					// If the closest point is not on the arcs, then we project it on the legs or the cutoff line and pick the closest one.
					// Note that all these distances should be reduced by obstacleRadius, but we only compare the values, so this doesn't matter.
					float distToCutoff = (t > 1.0f || t < 0.0f || cutoffLength < 0.0001f ? math.INFINITY : math.lengthsq(optimalVelocity - (leftCutoff + t * cutoffDir)));
					float distToLeftLeg = (tLeft < 0.0f ? math.INFINITY : math.lengthsq(optimalVelocity - (leftCutoff + tLeft * leftLegDirection)));
					float distToRightLeg = (tRight < 0.0f ? math.INFINITY : math.lengthsq(optimalVelocity - (rightCutoff + tRight * rightLegDirection)));
					var selected = 0;
					var mn = distToCutoff;
					if (distToLeftLeg < mn) {
						mn = distToLeftLeg;
						selected = 1;
					}
					if (distToRightLeg < mn) {
						mn = distToRightLeg;
						selected = 2;
					}

					if (selected == 0) {
						// Project on cutoff line
						orcaLineToAgent[numLines] = -1;
						orcaLines[numLines++] = new ORCALine {
							point = leftCutoff + obstacleRadius * inverseObstacleTimeHorizon * new float2(segmentDir.y, -segmentDir.x),
							direction = -segmentDir,
						};
					} else if (selected == 1) {
						if (!isLeftLegForeign) {
							orcaLineToAgent[numLines] = -1;
							orcaLines[numLines++] = new ORCALine {
								point = leftCutoff + obstacleRadius * inverseObstacleTimeHorizon * new float2(-leftLegDirection.y, leftLegDirection.x),
								direction = leftLegDirection,
							};
						}
					} else if (selected == 2) {
						if (!isRightLegForeign) {
							orcaLineToAgent[numLines] = -1;
							orcaLines[numLines++] = new ORCALine {
								point = rightCutoff + obstacleRadius * inverseObstacleTimeHorizon * new float2(rightLegDirection.y, -rightLegDirection.x),
								direction = -rightLegDirection,
							};
						}
					}
				}
				MarkerConvertObstacles2.End();
			}
		}

		public void ExecuteORCA (int startIndex, int batchSize) {
			int endIndex = startIndex + batchSize;

			NativeArray<ORCALine> orcaLines = new NativeArray<ORCALine>(SimulatorBurst.MaxNeighbourCount + MaxObstacleCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			NativeArray<ORCALine> scratchBuffer = new NativeArray<ORCALine>(SimulatorBurst.MaxNeighbourCount + MaxObstacleCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			NativeArray<float> segmentDistancesScratch = new NativeArray<float>(SimulatorBurst.MaxObstacleVertices, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			NativeArray<int> sortedVerticesScratch = new NativeArray<int>(SimulatorBurst.MaxObstacleVertices, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			NativeArray<int2> adjacentObstacleVertices = new NativeArray<int2>(4 * SimulatorBurst.MaxObstacleVertices, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			NativeArray<int> orcaLineToAgent = new NativeArray<int>(SimulatorBurst.MaxNeighbourCount + MaxObstacleCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
			NativeList<int> adjacentObstacleIdsScratch = new NativeList<int>(16, Allocator.Temp);

			for (int agentIndex = startIndex; agentIndex < endIndex; agentIndex++) {
				if (!agentData.version[agentIndex].Valid) continue;

				if (agentData.manuallyControlled[agentIndex]) {
					output.speed[agentIndex] = agentData.desiredSpeed[agentIndex];
					output.targetPoint[agentIndex] = agentData.targetPoint[agentIndex];
					output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount] = -1;
					continue;
				}

				var position = agentData.position[agentIndex];

				if (agentData.locked[agentIndex]) {
					output.speed[agentIndex] = 0;
					output.targetPoint[agentIndex] = position;
					output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount] = -1;
					continue;
				}

				MovementPlaneWrapper movementPlane = default;
				movementPlane.Set(agentData.movementPlane[agentIndex]);

				// The RVO algorithm assumes we will continue to
				// move in roughly the same direction
				float2 optimalVelocity = movementPlane.ToPlane(temporaryAgentData.currentVelocity[agentIndex]);
				int numLines = 0;
				// TODO: Obstacles are typically behind agents, so it's better to add the agent orca lines first to improve culling.
				// However, the 3D optimization program requires obstacle lines to be added first. Not to mention that the culling
				// is not strictly accurate for fixed obstacle since they cannot be moved backwards by the 3D linear program.
				GenerateObstacleVOs(agentIndex, adjacentObstacleIdsScratch, adjacentObstacleVertices, segmentDistancesScratch, sortedVerticesScratch, orcaLines, orcaLineToAgent, ref numLines, in movementPlane, optimalVelocity);
				int numFixedLines = numLines;

				var neighbours = temporaryAgentData.neighbours.Slice(agentIndex*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);

				float agentTimeHorizon = agentData.agentTimeHorizon[agentIndex];
				float inverseAgentTimeHorizon = math.rcp(agentTimeHorizon);
				float priority = agentData.priority[agentIndex];

				var localPosition = movementPlane.ToPlane(position);
				var agentRadius = agentData.radius[agentIndex];

				for (int neighbourIndex = 0; neighbourIndex < neighbours.Length; neighbourIndex++) {
					int otherIndex = neighbours[neighbourIndex];
					// Indicates that there are no more neighbours (see JobRVOCalculateNeighbours)
					if (otherIndex == -1) break;

					var otherPosition = agentData.position[otherIndex];
					var relativePosition = movementPlane.ToPlane(otherPosition - position);
					float combinedRadius = agentRadius + agentData.radius[otherIndex];

					var otherPriority = agentData.priority[otherIndex] * priorityMultiplier;

					// TODO: Remove branches to possibly vectorize
					float avoidanceStrength;
					if (agentData.locked[otherIndex] || agentData.manuallyControlled[otherIndex]) {
						avoidanceStrength = 1;
					} else if (otherPriority > 0.00001f || priority > 0.00001f) {
						avoidanceStrength = otherPriority / (priority + otherPriority);
					} else {
						// Both this agent's priority and the other agent's priority is zero or negative
						// Assume they have the same priority
						avoidanceStrength = 0.5f;
					}

					// We assume that the other agent will continue to move with roughly the same velocity if the priorities for the agents are similar.
					// If the other agent has a higher priority than this agent (avoidanceStrength > 0.5) then we will assume it will move more along its
					// desired velocity. This will have the effect of other agents trying to clear a path for where a high priority agent wants to go.
					// If this is not done then even high priority agents can get stuck when it is really crowded and they have had to slow down.
					float2 otherOptimalVelocity = movementPlane.ToPlane(math.lerp(temporaryAgentData.currentVelocity[otherIndex], temporaryAgentData.desiredVelocity[otherIndex], math.clamp(2*avoidanceStrength - 1, 0, 1)));

					if (agentData.flowFollowingStrength[otherIndex] > 0) {
						// When flow following strength is 1 the component of the other agent's velocity that is in the direction of this agent is removed.
						// That is, we pretend that the other agent does not move towards this agent at all.
						// This will make it impossible for the other agent to "push" this agent away.
						var strength = agentData.flowFollowingStrength[otherIndex] * agentData.flowFollowingStrength[agentIndex];
						var relativeDir = math.normalizesafe(relativePosition);
						otherOptimalVelocity -= relativeDir * (strength * math.min(0, math.dot(otherOptimalVelocity, relativeDir)));
					}

					var dist = math.length(relativePosition);
					// Figure out an approximate time to collision. We avoid using the current velocities of the agents because that leads to oscillations,
					// as the agents change their velocities, which results in a change to the time to collision, which makes them change their velocities again.
					var minimumTimeToCollision = math.max(0, dist - combinedRadius) / math.max(combinedRadius, agentData.desiredSpeed[agentIndex] + agentData.desiredSpeed[otherIndex]);

					// Adjust the radius to make the avoidance smoother.
					// The agent will slowly start to take another agent into account instead of making a sharp turn.
					float normalizedTime = minimumTimeToCollision * inverseAgentTimeHorizon;
					// normalizedTime <= 0.5 => 0% effect
					// normalizedTime  = 1.0 => 100% effect
					var factor = math.clamp((normalizedTime - 0.5f)*2.0f, 0, 1);
					combinedRadius *= 1 - factor;

					// Adjust the time horizon to make the agent approach another agent less conservatively.
					// This makes the velocity curve closer to sqrt(1-t) instead of exp(-t) as it comes to a stop, which looks nicer.
					var tempInverseTimeHorizon = 1.0f/math.max(0.1f*agentTimeHorizon, agentTimeHorizon * math.clamp(math.sqrt(2f*minimumTimeToCollision), 0, 1));

					orcaLines[numLines] = new ORCALine(localPosition, relativePosition, optimalVelocity, otherOptimalVelocity, combinedRadius, 0.1f, tempInverseTimeHorizon);
					orcaLineToAgent[numLines] = otherIndex;
					numLines++;
#if UNITY_EDITOR
					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.AgentVOs)) {
						draw.PushMatrix(math.mul(float4x4.TRS(position, quaternion.identity, 1), movementPlane.matrix));
						var voCenter = math.lerp(optimalVelocity, otherOptimalVelocity, 0.5f);
						DrawVO(draw, relativePosition * tempInverseTimeHorizon + otherOptimalVelocity, combinedRadius * tempInverseTimeHorizon, otherOptimalVelocity, Color.black);
						draw.PopMatrix();
					}
#endif
				}

				// Add an obstacle for the collision normal.
				// This is mostly deprecated, but kept for compatibility.
				var collisionNormal = math.normalizesafe(movementPlane.ToPlane(agentData.collisionNormal[agentIndex]));
				if (math.any(collisionNormal != 0)) {
					orcaLines[numLines] = new ORCALine {
						point = float2.zero,
						direction = new float2(collisionNormal.y, -collisionNormal.x),
					};
					orcaLineToAgent[numLines] = -1;
					numLines++;
				}

				var desiredVelocity = movementPlane.ToPlane(temporaryAgentData.desiredVelocity[agentIndex]);
				var desiredTargetPointInVelocitySpace = temporaryAgentData.desiredTargetPointInVelocitySpace[agentIndex];
				var originalDesiredVelocity = desiredVelocity;
				var symmetryBias = symmetryBreakingBias * (1 - agentData.flowFollowingStrength[agentIndex]);
				// Bias the desired velocity to avoid symmetry issues (esp. when two agents are heading straight towards one another).
				// Do not bias velocities if the agent is heading towards an obstacle (not an agent).
				bool insideAnyVO = BiasDesiredVelocity(orcaLines.AsUnsafeSpan().Slice(numFixedLines, numLines - numFixedLines), ref desiredVelocity, ref desiredTargetPointInVelocitySpace, symmetryBias);
				// If the velocity is outside all agent orca half-planes, do a more thorough check of all orca lines (including obstacles).
				insideAnyVO = insideAnyVO || DistanceInsideVOs(orcaLines.AsUnsafeSpan().Slice(0, numLines), desiredVelocity) > 0;


#if UNITY_EDITOR
				if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.ObstacleVOs)) {
					draw.PushColor(new Color(1, 1, 1, 0.2f));
					draw.PushMatrix(math.mul(float4x4.TRS(position, quaternion.identity, 1), movementPlane.matrix));
					for (int i = 0; i < numLines; i++) {
						orcaLines[i].DrawAsHalfPlane(draw, agentData.radius[agentIndex] * 5.0f, 1.0f, i >= numFixedLines ? Color.magenta : Color.Lerp(Color.magenta, Color.black, 0.5f));
					}
					draw.PopMatrix();
					draw.PopColor();
				}
#endif

				if (!insideAnyVO && math.all(math.abs(temporaryAgentData.collisionVelocityOffsets[agentIndex]) < 0.001f)) {
					// Desired velocity can be used directly since it was not inside any velocity obstacle.
					// No need to run optimizer because this will be the global minima.
					// This is also a special case in which we can set the
					// calculated target point to the desired target point
					// instead of calculating a point based on a calculated velocity
					// which is an important difference when the agent is very close
					// to the target point
					// TODO: Not actually guaranteed to be global minima if desiredTargetPointInVelocitySpace.magnitude < desiredSpeed
					// maybe do something different here?
#if UNITY_EDITOR
					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.DesiredVelocity)) {
						draw.xy.Cross(movementPlane.ToWorld(localPosition + desiredVelocity), Color.magenta);
						draw.xy.Cross(movementPlane.ToWorld(localPosition + desiredTargetPointInVelocitySpace), Color.yellow);
					}
#endif

					output.targetPoint[agentIndex] = position + movementPlane.ToWorld(desiredTargetPointInVelocitySpace, 0);
					output.speed[agentIndex] = agentData.desiredSpeed[agentIndex];
					output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount] = -1;
					output.forwardClearance[agentIndex] = float.PositiveInfinity;
				} else {
					var maxSpeed = agentData.maxSpeed[agentIndex];
					var allowedVelocityDeviationAngles = agentData.allowedVelocityDeviationAngles[agentIndex];
					LinearProgram2Output lin;
					if (math.all(allowedVelocityDeviationAngles == 0)) {
						// Common case, the desired velocity is a point
						lin = LinearProgram2D(orcaLines, numLines, maxSpeed, desiredVelocity, false);
					} else {
						// The desired velocity is a segment, not a point

						// Rotate the desired velocity allowedVelocityDeviationAngles.x radians and allowedVelocityDeviationAngles.y radians respectively
						math.sincos(allowedVelocityDeviationAngles, out float2 s, out float2 c);
						var xs = desiredVelocity.x*c - desiredVelocity.y*s;
						var ys = desiredVelocity.x*s + desiredVelocity.y*c;
						var desiredVelocityLeft = new float2(xs.x, ys.x);
						var desiredVelocityRight = new float2(xs.y, ys.y);

						var desiredVelocityLeftDir = desiredVelocity - desiredVelocityLeft;

						// Normalize and store length
						var desiredVelocityLeftSegmentLength = math.length(desiredVelocityLeftDir);
						desiredVelocityLeftDir = math.select(float2.zero, desiredVelocityLeftDir * math.rcp(desiredVelocityLeftSegmentLength), desiredVelocityLeftSegmentLength > math.FLT_MIN_NORMAL);

						var desiredVelocityRightDir = desiredVelocity - desiredVelocityRight;
						var desiredVelocityRightSegmentLength = math.length(desiredVelocityRightDir);
						desiredVelocityRightDir = math.select(float2.zero, desiredVelocityRightDir * math.rcp(desiredVelocityRightSegmentLength), desiredVelocityRightSegmentLength > math.FLT_MIN_NORMAL);

						// var tOptimal = ClosestPointOnSegment(desiredVelocityLeft, desiredVelocityDir, desiredVelocity, 0, desiredVelocitySegmentLength);

						var lin1 = LinearProgram2DSegment(orcaLines, numLines, maxSpeed, desiredVelocityLeft, desiredVelocityLeftDir, 0, desiredVelocityLeftSegmentLength, 1.0f);
						var lin2 = LinearProgram2DSegment(orcaLines, numLines, maxSpeed, desiredVelocityRight, desiredVelocityRightDir, 0, desiredVelocityRightSegmentLength, 1.0f);

						if (lin1.firstFailedLineIndex < lin2.firstFailedLineIndex) {
							lin = lin1;
						} else if (lin2.firstFailedLineIndex < lin1.firstFailedLineIndex) {
							lin = lin2;
						} else {
							lin = math.lengthsq(lin1.velocity - desiredVelocity) < math.lengthsq(lin2.velocity - desiredVelocity) ? lin1 : lin2;
						}
					}

					float2 newVelocity;
					if (lin.firstFailedLineIndex < numLines) {
						newVelocity = lin.velocity;
						LinearProgram3D(orcaLines, numLines, numFixedLines, lin.firstFailedLineIndex, maxSpeed, ref newVelocity, scratchBuffer);
					} else {
						newVelocity = lin.velocity;
					}

#if UNITY_EDITOR
					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.ChosenVelocity)) {
						draw.xy.Cross(position + movementPlane.ToWorld(newVelocity), Color.white);
						draw.Arrow(position + movementPlane.ToWorld(desiredVelocity), position + movementPlane.ToWorld(newVelocity), Color.magenta);
					}
#endif

					var blockedByAgentCount = 0;
					for (int i = 0; i < numLines && blockedByAgentCount < SimulatorBurst.MaxBlockingAgentCount; i++) {
						if (orcaLineToAgent[i] != -1 && det(orcaLines[i].direction, orcaLines[i].point - newVelocity) >= -0.001f) {
							// We are blocked by this line
							output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + blockedByAgentCount] = orcaLineToAgent[i];
							blockedByAgentCount++;
						}
					}
					if (blockedByAgentCount < SimulatorBurst.MaxBlockingAgentCount) output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + blockedByAgentCount] = -1;

					var collisionVelocityOffset = temporaryAgentData.collisionVelocityOffsets[agentIndex];
					if (math.any(collisionVelocityOffset != 0)) {
						// Make the agent move to avoid intersecting other agents (hard collisions)
						newVelocity += temporaryAgentData.collisionVelocityOffsets[agentIndex];

						// Adding the collision offset may have made the velocity invalid, causing it to intersect the wall-velocity-obstacles.
						// We run a second optimization on only the wall-velocity-obstacles to make sure the velocity is valid.
						newVelocity = LinearProgram2D(orcaLines, numFixedLines, maxSpeed, newVelocity, false).velocity;
					}

					output.targetPoint[agentIndex] = position + movementPlane.ToWorld(newVelocity, 0);
					output.speed[agentIndex] = math.min(math.length(newVelocity), maxSpeed);

					var targetDir = math.normalizesafe(movementPlane.ToPlane(agentData.targetPoint[agentIndex] - position));
					var forwardClearance = CalculateForwardClearance(neighbours, movementPlane, position, agentRadius, targetDir);
					output.forwardClearance[agentIndex] = forwardClearance;
					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.ForwardClearance) && forwardClearance < float.PositiveInfinity) {
						draw.PushLineWidth(2);
						draw.Ray(position, movementPlane.ToWorld(targetDir) * forwardClearance, Color.red);
						draw.PopLineWidth();
					}
				}
			}
		}

		/// <summary>
		/// Find the distance we can move towards our target without colliding with anything.
		/// May become negative if we are currently colliding with something.
		/// </summary>
		float CalculateForwardClearance (NativeSlice<int> neighbours, MovementPlaneWrapper movementPlane, float3 position, float radius, float2 targetDir) {
			// TODO: Take obstacles into account.
			var smallestIntersectionDistance = float.PositiveInfinity;
			for (int i = 0; i < neighbours.Length; i++) {
				var other = neighbours[i];
				if (other == -1) break;
				var otherPosition = agentData.position[other];
				var combinedRadius = radius + agentData.radius[other];
				// Intersect the ray from our agent towards the destination and check the distance to the intersection with the other agent.
				var otherDir = movementPlane.ToPlane(otherPosition - position);
				// Squared cosine of the angle between otherDir and ourTargetDir
				var cosAlpha = math.dot(math.normalizesafe(otherDir), targetDir);

				// Check if the agent is behind us
				if (cosAlpha < 0) continue;

				var distToOtherSq = math.lengthsq(otherDir);
				var distToClosestPointAlongRay = math.sqrt(distToOtherSq) * cosAlpha;
				var discriminant = combinedRadius*combinedRadius - (distToOtherSq - distToClosestPointAlongRay*distToClosestPointAlongRay);

				// Check if we have any intersection at all
				if (discriminant < 0) continue;
				var distToIntersection = distToClosestPointAlongRay - math.sqrt(discriminant);
				smallestIntersectionDistance = math.min(smallestIntersectionDistance, distToIntersection);
			}
			return smallestIntersectionDistance;
		}

		/// <summary>True if vector2 is to the left of vector1 or if they are colinear.</summary>
		static bool leftOrColinear (float2 vector1, float2 vector2) {
			return det(vector1, vector2) >= 0;
		}

		/// <summary>True if vector2 is to the left of vector1.</summary>
		static bool left (float2 vector1, float2 vector2) {
			return det(vector1, vector2) > 0;
		}

		/// <summary>True if vector2 is to the right of vector1 or if they are colinear.</summary>
		static bool rightOrColinear (float2 vector1, float2 vector2) {
			return det(vector1, vector2) <= 0;
		}

		/// <summary>True if vector2 is to the right of vector1.</summary>
		static bool right (float2 vector1, float2 vector2) {
			return det(vector1, vector2) < 0;
		}

		/// <summary>
		/// Determinant of the 2x2 matrix defined by vector1 and vector2.
		/// Alternatively, the Z component of the cross product of vector1 and vector2.
		/// </summary>
		static float det (float2 vector1, float2 vector2) {
			return vector1.x * vector2.y - vector1.y * vector2.x;
		}

		static float2 rot90 (float2 v) {
			return new float2(-v.y, v.x);
		}

		/// <summary>
		/// A half-plane defined as the line splitting plane.
		///
		/// For ORCA purposes, the infeasible region of the half-plane is on the right side of the line.
		/// </summary>
		struct ORCALine {
			public float2 point;
			public float2 direction;

			public void DrawAsHalfPlane (CommandBuilder draw, float halfPlaneLength, float halfPlaneWidth, Color color) {
				var normal = new float2(direction.y, -direction.x);
				draw.xy.Line(point - direction*10, point + direction*10, color);

				var p = point + normal*halfPlaneWidth*0.5f;
				draw.SolidBox(new float3(p, 0), quaternion.RotateZ(math.atan2(direction.y, direction.x)), new float3(halfPlaneLength, halfPlaneWidth, 0.01f), new Color(0, 0, 0, 0.5f));
			}

			public ORCALine(float2 position, float2 relativePosition, float2 velocity, float2 otherVelocity, float combinedRadius, float timeStep, float invTimeHorizon) {
				var relativeVelocity = velocity - otherVelocity;
				float combinedRadiusSq = combinedRadius*combinedRadius;
				float distSq = math.lengthsq(relativePosition);

				if (distSq > combinedRadiusSq) {
					combinedRadius *= 1.001f;
					// No collision

					// A velocity obstacle is built which is shaped like a truncated cone (see ORCA paper).
					// The cone is truncated by an arc centered at relativePosition/timeHorizon
					// with radius combinedRadius/timeHorizon.
					// The cone extends in the direction of relativePosition.

					// Vector from truncation arc center to relative velocity
					var w = relativeVelocity - invTimeHorizon * relativePosition;
					var wLengthSq = math.lengthsq(w);

					float dot1 = math.dot(w, relativePosition);

					if (dot1 < 0.0f && dot1*dot1 > combinedRadiusSq * wLengthSq) {
						// Project on cut-off circle
						float wLength = math.sqrt(wLengthSq);
						var normalizedW = w / wLength;

						direction = new float2(normalizedW.y, -normalizedW.x);
						var u = (combinedRadius * invTimeHorizon - wLength) * normalizedW;
						point = velocity + 0.5f * u;
					} else {
						// Project on legs
						// Distance from the agent to the point where the "legs" start on the VO
						float legDistance = math.sqrt(distSq - combinedRadiusSq);

						if (det(relativePosition, w) > 0.0f) {
							// Project on left leg
							// Note: This vector is actually normalized
							direction = (relativePosition * legDistance + new float2(-relativePosition.y, relativePosition.x) * combinedRadius) / distSq;
						} else {
							// Project on right leg
							// Note: This vector is actually normalized
							direction = (-relativePosition * legDistance + new float2(-relativePosition.y, relativePosition.x) * combinedRadius) / distSq;
						}

						float dot2 = math.dot(relativeVelocity, direction);
						var u = dot2 * direction - relativeVelocity;
						point = velocity + 0.5f * u;
					}
				} else {
					float invTimeStep = math.rcp(timeStep);
					var dist = math.sqrt(distSq);
					var normalizedDir = math.select(0, relativePosition / dist, dist > math.FLT_MIN_NORMAL);
					var u = normalizedDir * (dist - combinedRadius - 0.001f) * 0.3f * invTimeStep;
					direction = math.normalizesafe(new float2(u.y, -u.x));
					point = math.lerp(velocity, otherVelocity, 0.5f) + u * 0.5f;


					// Original code, the above is a version which works better
					// Collision
					// Project on cut-off circle of timeStep
					//float invTimeStep = 1.0f / timeStep;
					// Vector from cutoff center to relative velocity
					//float2 w = relativeVelocity - invTimeStep * relativePosition;
					//float wLength = math.length(w);
					//float2 unitW = w / wLength;
					//direction = new float2(unitW.y, -unitW.x);
					//var u = (combinedRadius * invTimeStep - wLength) * unitW;
					//point = velocity + 0.5f * u;
				}
			}
		}

		/// <summary>
		/// Calculates how far inside the infeasible region of the ORCA half-planes the velocity is.
		/// Returns 0 if the velocity is in the feasible region of all half-planes.
		/// </summary>
		static float DistanceInsideVOs (UnsafeSpan<ORCALine> lines, float2 velocity) {
			float maxDistance = 0.0f;

			for (int i = 0; i < lines.Length; i++) {
				var distance = det(lines[i].direction, lines[i].point - velocity);
				maxDistance = math.max(maxDistance, distance);
			}

			return maxDistance;
		}

		/// <summary>
		/// Bias towards the right side of agents.
		/// Rotate desiredVelocity at most [value] number of radians. 1 radian ≈ 57°
		/// This breaks up symmetries.
		///
		/// The desired velocity will only be rotated if it is inside a velocity obstacle (VO).
		/// If it is inside one, it will not be rotated further than to the edge of it
		///
		/// The targetPointInVelocitySpace will be rotated by the same amount as the desired velocity
		///
		/// Returns: True if the desired velocity was inside any VO
		/// </summary>
		static bool BiasDesiredVelocity (UnsafeSpan<ORCALine> lines, ref float2 desiredVelocity, ref float2 targetPointInVelocitySpace, float maxBiasRadians) {
			float maxDistance = DistanceInsideVOs(lines, desiredVelocity);

			if (maxDistance == 0.0f) return false;

			var desiredVelocityMagn = math.length(desiredVelocity);

			// Avoid division by zero below
			if (desiredVelocityMagn >= 0.001f) {
				// Rotate the desired velocity clockwise (to the right) at most maxBiasRadians number of radians.
				// We clamp the angle so that we do not rotate more than to the edge of the VO.
				// Assuming maxBiasRadians is small, we can just move it instead and it will give approximately the same effect.
				// See https://en.wikipedia.org/wiki/Small-angle_approximation
				var angle = math.min(maxBiasRadians, maxDistance / desiredVelocityMagn);
				desiredVelocity += new float2(desiredVelocity.y, -desiredVelocity.x) * angle;
				targetPointInVelocitySpace += new float2(targetPointInVelocitySpace.y, -targetPointInVelocitySpace.x) * angle;
			}
			return true;
		}

		/// <summary>
		/// Clip a line to the feasible region of the half-plane given by the clipper.
		/// The clipped line is `line.point + line.direction*tLeft` to `line.point + line.direction*tRight`.
		///
		/// Returns false if the line is parallel to the clipper's border.
		/// </summary>
		static bool ClipLine (ORCALine line, ORCALine clipper, ref float tLeft, ref float tRight) {
			float denominator = det(line.direction, clipper.direction);
			float numerator = det(clipper.direction, line.point - clipper.point);

			if (math.abs(denominator) < 0.0001f) {
				// The two lines are almost parallel
				return false;
			}

			float t = numerator / denominator;

			if (denominator >= 0.0f) {
				// Line i bounds the line on the right
				tRight = math.min(tRight, t);
			} else {
				// Line i bounds the line on the left
				tLeft = math.max(tLeft, t);
			}
			return true;
		}

		static bool ClipBoundary (NativeArray<ORCALine> lines, int lineIndex, float radius, out float tLeft, out float tRight) {
			var line = lines[lineIndex];
			if (!VectorMath.LineCircleIntersectionFactors(line.point, line.direction, radius, out tLeft, out tRight)) {
				return false;
			}

			// Go through all previous lines/half-planes and clip the current line against them
			for (int i = 0; i < lineIndex; i++) {
				float denominator = det(line.direction, lines[i].direction);
				float numerator = det(lines[i].direction, line.point - lines[i].point);

				if (math.abs(denominator) < 0.0001f) {
					// The two lines are almost parallel
					if (numerator < 0.0f) {
						// This line is completely "behind" the other line. So we can ignore it.
						return false;
					} else continue;
				}

				float t = numerator / denominator;

				if (denominator >= 0.0f) {
					// Line i bounds the line on the right
					tRight = math.min(tRight, t);
				} else {
					// Line i bounds the line on the left
					tLeft = math.max(tLeft, t);
				}

				if (tLeft > tRight) {
					// The line is completely outside the previous half-planes
					return false;
				}
			}
			return true;
		}

		static bool LinearProgram1D (NativeArray<ORCALine> lines, int lineIndex, float radius, float2 optimalVelocity, bool directionOpt, ref float2 result) {
			if (!ClipBoundary(lines, lineIndex, radius, out float tLeft, out float tRight)) return false;
			var line = lines[lineIndex];

			if (directionOpt) {
				// Optimize direction
				if (math.dot(optimalVelocity, line.direction) > 0.0f) {
					// Take right extreme
					result = line.point + tRight * line.direction;
				} else {
					// Take left extreme
					result = line.point + tLeft * line.direction;
				}
			} else {
				// Optimize closest point
				float t = math.dot(line.direction, optimalVelocity - line.point);
				result = line.point + math.clamp(t, tLeft, tRight) * line.direction;
			}
			return true;
		}

		struct LinearProgram2Output {
			public float2 velocity;
			public int firstFailedLineIndex;
		}

		static LinearProgram2Output LinearProgram2D (NativeArray<ORCALine> lines, int numLines, float radius, float2 optimalVelocity, bool directionOpt) {
			float2 result;

			if (directionOpt) {
				// Optimize direction. Note that the optimization velocity is of unit length in this case
				result = optimalVelocity * radius;
			} else if (math.lengthsq(optimalVelocity) > radius*radius) {
				// Optimize closest point and outside circle
				result = math.normalize(optimalVelocity) * radius;
			} else {
				// Optimize closest point and inside circle
				result = optimalVelocity;
			}

			for (int i = 0; i < numLines; i++) {
				// Check if point is in the infeasible region of the half-plane
				if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
					// Result does not satisfy constraint i. Compute new optimal result
					var tempResult = result;
					if (!LinearProgram1D(lines, i, radius, optimalVelocity, directionOpt, ref result)) {
						return new LinearProgram2Output {
								   velocity = tempResult,
								   firstFailedLineIndex = i,
						};
					}
				}
			}

			return new LinearProgram2Output {
					   velocity = result,
					   firstFailedLineIndex = numLines,
			};
		}

		static float ClosestPointOnSegment (float2 a, float2 dir, float2 p, float t0, float t1) {
			return math.clamp(math.dot(p - a, dir), t0, t1);
		}

		/// <summary>
		/// Closest point on segment a to segment b.
		/// The segments are given by infinite lines and bounded by t values. p = line.point + line.dir*t.
		///
		/// It is assumed that the two segments do not intersect.
		/// </summary>
		static float2 ClosestSegmentSegmentPointNonIntersecting (ORCALine a, ORCALine b, float ta1, float ta2, float tb1, float tb2) {
			// We know that the two segments do not intersect, so at least one of the closest points
			// must be one of the line segment endpoints.
			var ap0 = a.point + a.direction*ta1;
			var ap1 = a.point + a.direction*ta2;
			var bp0 = b.point + b.direction * tb1;
			var bp1 = b.point + b.direction * tb2;

			var t0 = ClosestPointOnSegment(a.point, a.direction, bp0, ta1, ta2);
			var t1 = ClosestPointOnSegment(a.point, a.direction, bp1, ta1, ta2);
			var t2 = ClosestPointOnSegment(b.point, b.direction, ap0, tb1, tb2);
			var t3 = ClosestPointOnSegment(b.point, b.direction, ap1, tb1, tb2);

			var c0 = a.point + a.direction * t0;
			var c1 = a.point + a.direction * t1;
			var c2 = b.point + b.direction * t2;
			var c3 = b.point + b.direction * t3;

			var d0 = math.lengthsq(c0 - bp0);
			var d1 = math.lengthsq(c1 - bp1);
			var d2 = math.lengthsq(c2 - ap0);
			var d3 = math.lengthsq(c3 - ap1);

			var result = c0;
			var d = d0;
			if (d1 < d) {
				result = c1;
				d = d1;
			}
			if (d2 < d) {
				result = ap0;
				d = d2;
			}
			if (d3 < d) {
				result = ap1;
				d = d3;
			}
			return result;
		}

		/// <summary>Like LinearProgram2D, but the optimal velocity space is a segment instead of a point, however the current result has collapsed to a point</summary>
		static LinearProgram2Output LinearProgram2DCollapsedSegment (NativeArray<ORCALine> lines, int numLines, int startLine, float radius, float2 currentResult, float2 optimalVelocityStart, float2 optimalVelocityDir, float optimalTLeft, float optimalTRight) {
			for (int i = startLine; i < numLines; i++) {
				// Check if point is in the infeasible region of the half-plane
				if (det(lines[i].direction, lines[i].point - currentResult) > 0.0f) {
					// Result does not satisfy constraint i. Compute new optimal result
					if (!ClipBoundary(lines, i, radius, out float tLeft2, out float tRight2)) {
						// We are partially not feasible, but no part of this constraint's boundary is in the feasible region.
						// This means that there is no feasible solution at all.
						return new LinearProgram2Output {
								   velocity = currentResult,
								   firstFailedLineIndex = i,
						};
					}

					// Optimize closest point
					currentResult = ClosestSegmentSegmentPointNonIntersecting(lines[i], new ORCALine {
						point = optimalVelocityStart,
						direction = optimalVelocityDir,
					}, tLeft2, tRight2, optimalTLeft, optimalTRight);
				}
			}

			return new LinearProgram2Output {
					   velocity = currentResult,
					   firstFailedLineIndex = numLines,
			};
		}

		/// <summary>Like LinearProgram2D, but the optimal velocity space is a segment instead of a point</summary>
		static LinearProgram2Output LinearProgram2DSegment (NativeArray<ORCALine> lines, int numLines, float radius, float2 optimalVelocityStart, float2 optimalVelocityDir, float optimalTLeft, float optimalTRight, float optimalT) {
			var hasIntersection = VectorMath.LineCircleIntersectionFactors(optimalVelocityStart, optimalVelocityDir, radius, out float resultTLeft, out float resultTRight);
			resultTLeft = math.max(resultTLeft, optimalTLeft);
			resultTRight = math.min(resultTRight, optimalTRight);
			hasIntersection &= resultTLeft <= resultTRight;

			if (!hasIntersection) {
				// In case the optimal velocity segment is not inside the max velocity circle, then collapse to a single optimal velocity which
				// is closest segment point to the circle
				var t = math.clamp(math.dot(-optimalVelocityStart, optimalVelocityDir), optimalTLeft, optimalTRight);
				var closestOnCircle = math.normalizesafe(optimalVelocityStart + optimalVelocityDir * t) * radius;

				// The best point is now a single point, not a segment.
				// So we can fall back to simpler code.
				return LinearProgram2DCollapsedSegment(lines, numLines, 0, radius, closestOnCircle, optimalVelocityStart, optimalVelocityDir, optimalTLeft, optimalTRight);
			}

			for (int i = 0; i < numLines; i++) {
				// Check if optimal line segment is at least partially in the infeasible region of the half-plane
				var line = lines[i];
				var leftInfeasible = det(line.direction, line.point - (optimalVelocityStart + optimalVelocityDir*resultTLeft)) > 0.0f;
				var rightInfeasible = det(line.direction, line.point - (optimalVelocityStart + optimalVelocityDir*resultTRight)) > 0.0f;
				if (leftInfeasible || rightInfeasible) {
					if (!ClipBoundary(lines, i, radius, out float tLeft, out float tRight)) {
						// We are partially not feasible, but no part of this constraint's boundary is in the feasible region.
						// This means that there is no feasible solution at all.
						return new LinearProgram2Output {
								   velocity = optimalVelocityStart + optimalVelocityDir * math.clamp(optimalT, resultTLeft, resultTRight),
								   firstFailedLineIndex = i,
						};
					}

					// Check if the optimal line segment is completely in the infeasible region
					if (leftInfeasible && rightInfeasible) {
						if (math.abs(det(line.direction, optimalVelocityDir)) < 0.001f) {
							// Lines are almost parallel.
							// Project the optimal velocity on the boundary
							var t1 = ClosestPointOnSegment(line.point, line.direction, optimalVelocityStart + optimalVelocityDir*resultTLeft, tLeft, tRight);
							var t2 = ClosestPointOnSegment(line.point, line.direction, optimalVelocityStart + optimalVelocityDir*resultTRight, tLeft, tRight);
							var t3 = ClosestPointOnSegment(line.point, line.direction, optimalVelocityStart + optimalVelocityDir*optimalT, tLeft, tRight);
							optimalVelocityStart = line.point;
							optimalVelocityDir = line.direction;
							resultTLeft = t1;
							resultTRight = t2;
							optimalT = t3;
						} else {
							// Find closest point on the constraint boundary segment to the optimal velocity segment
							var result = ClosestSegmentSegmentPointNonIntersecting(line, new ORCALine {
								point = optimalVelocityStart,
								direction = optimalVelocityDir,
							}, tLeft, tRight, optimalTLeft, optimalTRight);

							// The best point is now a single point, not a segment.
							// So we can fall back to simpler code.
							return LinearProgram2DCollapsedSegment(lines, numLines, i+1, radius, result, optimalVelocityStart, optimalVelocityDir, optimalTLeft, optimalTRight);
						}
					} else {
						// Clip optimal velocity segment to the constraint boundary.
						// If this returns false and the lines are almost parallel, then we don't do anything
						// because we already know they intersect. So the two lines must be almost identical.
						ClipLine(new ORCALine {
							point = optimalVelocityStart,
							direction = optimalVelocityDir,
						}, line, ref resultTLeft, ref resultTRight);
					}
				}
			}

			var resultT = math.clamp(optimalT, resultTLeft, resultTRight);

			return new LinearProgram2Output {
					   velocity = optimalVelocityStart + optimalVelocityDir * resultT,
					   firstFailedLineIndex = numLines,
			};
		}

		/// <summary>
		/// Finds the velocity with the smallest maximum penetration into the given half-planes.
		///
		/// Assumes there are no points in the feasible region of the given half-planes.
		///
		/// Runs a 3-dimensional linear program, but projected down to 2D.
		/// If there are no feasible regions outside all half-planes then we want to find the velocity
		/// for which the maximum penetration into infeasible regions is minimized.
		/// Conceptually we can solve this by taking our half-planes, and moving them outwards at a fixed speed
		/// until there is exactly 1 feasible point.
		/// We can formulate this in 3D space by thinking of the half-planes in 3D (velocity.x, velocity.y, penetration-depth) space, as sloped planes.
		/// Moving the planes outwards then corresponds to decreasing the z coordinate.
		/// In 3D space we want to find the point above all planes with the lowest z coordinate.
		/// We do this by going through each plane and testing if it is possible that this plane
		/// is the one with the maximum penetration.
		/// If so, we know that the point will lie on the portion of that plane bounded by the intersections
		/// with the other planes. We generate projected half-planes which represent the intersections with the
		/// other 3D planes, and then we run a new optimization to find the point which penetrates this
		/// half-plane the least.
		/// </summary>
		/// <param name="lines">The half-planes of all obstacles and agents.</param>
		/// <param name="numLines">The number of half-planes in lines.</param>
		/// <param name="numFixedLines">The number of half-planes in lines which are fixed (0..numFixedLines). These will be treated as static obstacles which should be avoided at all costs.</param>
		/// <param name="beginLine">The index of the first half-plane in lines for which the previous optimization failed (see \reflink{LinearProgram2Output.firstFailedLineIndex}).</param>
		/// <param name="radius">Maximum possible speed. This represents a circular velocity obstacle.</param>
		/// <param name="result">Input is best velocity as output by \reflink{LinearProgram2D}. Output is the new best velocity. The velocity with the smallest maximum penetration into the given half-planes.</param>
		/// <param name="scratchBuffer">A buffer of length at least numLines to use for scratch space.</param>
		static void LinearProgram3D (NativeArray<ORCALine> lines, int numLines, int numFixedLines, int beginLine, float radius, ref float2 result, NativeArray<ORCALine> scratchBuffer) {
			float distance = 0.0f;

			NativeArray<ORCALine> projectedLines = scratchBuffer;
			NativeArray<ORCALine>.Copy(lines, projectedLines, numFixedLines);

			for (int i = beginLine; i < numLines; i++) {
				// Check if #result is more than #distance units inside the infeasible region of the half-plane
				if (det(lines[i].direction, lines[i].point - result) > distance) {
					int numProjectedLines = numFixedLines;
					for (int j = numFixedLines; j < i; j++) {
						float determinant = det(lines[i].direction, lines[j].direction);
						if (math.abs(determinant) < 0.001f) {
							// Lines i and j are parallel
							if (math.dot(lines[i].direction, lines[j].direction) > 0.0f) {
								// Line i and j point in the same direction
								continue;
							} else {
								// Line i and j point in the opposite direction
								projectedLines[numProjectedLines] = new ORCALine {
									point = 0.5f * (lines[i].point + lines[j].point),
									direction = math.normalize(lines[j].direction - lines[i].direction),
								};
								numProjectedLines++;
							}
						} else {
							projectedLines[numProjectedLines] = new ORCALine {
								// The intersection between the two lines
								point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction,
								// The direction along which the intersection of the two 3D-planes intersect (projected onto the XY plane)
								direction = math.normalize(lines[j].direction - lines[i].direction),
							};
							numProjectedLines++;
						}
					}

					var lin = LinearProgram2D(projectedLines, numProjectedLines, radius, new float2(-lines[i].direction.y, lines[i].direction.x), true);
					if (lin.firstFailedLineIndex < numProjectedLines) {
						// This should in principle not happen.  The result is by definition
						// already in the feasible region of this linear program. If it fails,
						// it is due to small floating point error, and the current result is
						// kept.
					} else {
						result = lin.velocity;
					}

					distance = det(lines[i].direction, lines[i].point - result);
				}
			}
		}

		static void DrawVO (CommandBuilder draw, float2 circleCenter, float radius, float2 origin, Color color) {
#if UNITY_EDITOR
			draw.PushColor(color);
			float alpha = math.atan2((origin - circleCenter).y, (origin - circleCenter).x);
			float gamma = radius/math.length(origin-circleCenter);
			float delta = gamma <= 1.0f ? math.abs(math.acos(gamma)) : 0;

			draw.xy.Circle(circleCenter, radius, alpha-delta, alpha+delta);
			float2 p1 = new float2(math.cos(alpha-delta), math.sin(alpha-delta)) * radius;
			float2 p2 = new float2(math.cos(alpha+delta), math.sin(alpha+delta)) * radius;

			float2 p1t = -new float2(-p1.y, p1.x);
			float2 p2t = new float2(-p2.y, p2.x);
			p1 += circleCenter;
			p2 += circleCenter;

			draw.xy.Ray(p1, math.normalizesafe(p1t)*100);
			draw.xy.Ray(p2, math.normalizesafe(p2t)*100);
			draw.PopColor();
#endif
		}
	}
}