-----------------------------------PATHER--------------------------------------


local function Pather(endPoint, surfaceNormal, overrideUseDirectPath)
	local this = {}

	local directPathForHumanoid
	local directPathForVehicle
	if overrideUseDirectPath ~= nil then
		directPathForHumanoid = overrideUseDirectPath
		directPathForVehicle = overrideUseDirectPath
	else
		directPathForHumanoid = UseDirectPath
		directPathForVehicle = UseDirectPathForVehicle
	end

	this.Cancelled = false
	this.Started = false

	this.Finished = Instance.new("BindableEvent")
	this.PathFailed = Instance.new("BindableEvent")

	this.PathComputing = false
	this.PathComputed = false

	this.OriginalTargetPoint = endPoint
	this.TargetPoint = endPoint
	this.TargetSurfaceNormal = surfaceNormal

	this.DiedConn = nil
	this.SeatedConn = nil
	this.MoveToConn = nil	-- To be removd with FFlagUserClickToMoveFollowPathRefactor
	this.BlockedConn = nil
	this.TeleportedConn = nil

	this.CurrentPoint = 0

	this.HumanoidOffsetFromPath = ZERO_VECTOR3

	this.CurrentWaypointPosition = nil
	this.CurrentWaypointPlaneNormal = ZERO_VECTOR3
	this.CurrentWaypointPlaneDistance = 0
	this.CurrentWaypointNeedsJump = false;

	this.CurrentHumanoidPosition = ZERO_VECTOR3
	this.CurrentHumanoidVelocity = 0

	this.NextActionMoveDirection = ZERO_VECTOR3
	this.NextActionJump = false

	this.Timeout = 0

	this.Humanoid = findPlayerHumanoid(Player)
	this.OriginPoint = nil
	this.AgentCanFollowPath = false
	this.DirectPath = false
	this.DirectPathRiseFirst = false

	if FFlagUserClickToMoveFollowPathRefactor then
		local rootPart = this.Humanoid and this.Humanoid.RootPart
		if rootPart then
			-- Setup origin
			this.OriginPoint = rootPart.CFrame.p

			-- Setup agent
			local agentRadius = 2
			local agentHeight = 5
			local agentCanJump = true

			local seat = this.Humanoid.SeatPart
			if seat and seat:IsA("VehicleSeat") then
				-- Humanoid is seated on a vehicle
				local vehicle = seat:FindFirstAncestorOfClass("Model")
				if vehicle then
					-- Make sure the PrimaryPart is set to the vehicle seat while we compute the extends.
					local tempPrimaryPart = vehicle.PrimaryPart
					vehicle.PrimaryPart = seat

					-- For now, only direct path
					if directPathForVehicle then
						local extents = vehicle:GetExtentsSize()
						agentRadius = AgentSizeIncreaseFactor * 0.5 * math.sqrt(extents.X * extents.X + extents.Z * extents.Z)
						agentHeight = AgentSizeIncreaseFactor * extents.Y
						agentCanJump = false
						this.AgentCanFollowPath = true
						this.DirectPath = directPathForVehicle
					end

					-- Reset PrimaryPart
					vehicle.PrimaryPart = tempPrimaryPart
				end
			else
				local extents = GetCharacter():GetExtentsSize()
				agentRadius = AgentSizeIncreaseFactor * 0.5 * math.sqrt(extents.X * extents.X + extents.Z * extents.Z)
				agentHeight = AgentSizeIncreaseFactor * extents.Y
				agentCanJump = (this.Humanoid.JumpPower > 0)
				this.AgentCanFollowPath = true
				this.DirectPath = directPathForHumanoid
				this.DirectPathRiseFirst = this.Humanoid.Sit
			end

			-- Build path object
			this.pathResult = PathfindingService:CreatePath({AgentRadius = agentRadius, AgentHeight = agentHeight, AgentCanJump = agentCanJump})
		end
	else
		if this.Humanoid then
			local torso = this.Humanoid.Torso
			if torso then
				this.OriginPoint = torso.CFrame.p
				this.AgentCanFollowPath = true
				this.DirectPath = directPathForHumanoid
			end
		end
	end

	function this:Cleanup()
		if this.stopTraverseFunc then
			this.stopTraverseFunc()
			this.stopTraverseFunc = nil
		end

		if this.MoveToConn then
			this.MoveToConn:Disconnect()
			this.MoveToConn = nil
		end

		if this.BlockedConn then
			this.BlockedConn:Disconnect()
			this.BlockedConn = nil
		end

		if this.DiedConn then
			this.DiedConn:Disconnect()
			this.DiedConn = nil
		end

		if this.SeatedConn then
			this.SeatedConn:Disconnect()
			this.SeatedConn = nil
		end

		if this.TeleportedConn then
			this.TeleportedConn:Disconnect()
			this.TeleportedConn = nil
		end

		this.Started = false
	end

	function this:Cancel()
		this.Cancelled = true
		this:Cleanup()
	end

	function this:IsActive()
		return this.AgentCanFollowPath and this.Started and not this.Cancelled
	end

	function this:OnPathInterrupted()
		-- Stop moving
		this.Cancelled = true
		this:OnPointReached(false)
	end

	function this:ComputePath()
		if this.OriginPoint then
			if this.PathComputed or this.PathComputing then return end
			this.PathComputing = true
			if FFlagUserClickToMoveFollowPathRefactor then
				if this.AgentCanFollowPath then
					if this.DirectPath then
						this.pointList = {
							PathWaypoint.new(this.OriginPoint, Enum.PathWaypointAction.Walk),
							PathWaypoint.new(this.TargetPoint, this.DirectPathRiseFirst and Enum.PathWaypointAction.Jump or Enum.PathWaypointAction.Walk)
						}
						this.PathComputed = true
					else
						this.pathResult:ComputeAsync(this.OriginPoint, this.TargetPoint)
						this.pointList = this.pathResult:GetWaypoints()
						this.BlockedConn = this.pathResult.Blocked:Connect(function(blockedIdx) this:OnPathBlocked(blockedIdx) end)
						this.PathComputed = this.pathResult.Status == Enum.PathStatus.Success
					end
				end
			else
				pcall(function()
					this.pathResult = PathfindingService:FindPathAsync(this.OriginPoint, this.TargetPoint)
				end)
				this.pointList = this.pathResult and this.pathResult:GetWaypoints()
				if this.pathResult then
					this.BlockedConn = this.pathResult.Blocked:Connect(function(blockedIdx) this:OnPathBlocked(blockedIdx) end)
				end
				this.PathComputed = this.pathResult and this.pathResult.Status == Enum.PathStatus.Success or false
			end
			this.PathComputing = false
		end
	end

	function this:IsValidPath()
		if FFlagUserClickToMoveFollowPathRefactor then
			this:ComputePath()
			return this.PathComputed and this.AgentCanFollowPath
		else
			if not this.pathResult then
				this:ComputePath()
			end
			return this.pathResult.Status == Enum.PathStatus.Success
		end
	end

	this.Recomputing = false
	function this:OnPathBlocked(blockedWaypointIdx)
		local pathBlocked = blockedWaypointIdx >= this.CurrentPoint
		if not pathBlocked or this.Recomputing then
			return
		end

		this.Recomputing = true

		if this.stopTraverseFunc then
			this.stopTraverseFunc()
			this.stopTraverseFunc = nil
		end

		if FFlagUserClickToMoveFollowPathRefactor then
			this.OriginPoint = this.Humanoid.RootPart.CFrame.p
		else
			this.OriginPoint = this.Humanoid.Torso.CFrame.p
		end

		this.pathResult:ComputeAsync(this.OriginPoint, this.TargetPoint)
		this.pointList = this.pathResult:GetWaypoints()
		if #this.pointList > 0 then
			if FFlagUserClickToMoveFollowPathRefactor then
				this.HumanoidOffsetFromPath = this.pointList[1].Position - this.OriginPoint
			else
				-- Nothing to do : offset did not change
			end
		end
		this.PathComputed = this.pathResult.Status == Enum.PathStatus.Success

		if ShowPath then
			this.stopTraverseFunc, this.setPointFunc = ClickToMoveDisplay.CreatePathDisplay(this.pointList)
		end
		if this.PathComputed then
			this.CurrentPoint = 1 -- The first waypoint is always the start location. Skip it.
			this:OnPointReached(true) -- Move to first point
		else
			this.PathFailed:Fire()
			this:Cleanup()
		end

		this.Recomputing = false
	end

	function this:OnRenderStepped(dt)
		if this.Started and not this.Cancelled then
			-- Check for Timeout (if a waypoint is not reached within the delay, we fail)
			this.Timeout = this.Timeout + dt
			if this.Timeout > UnreachableWaypointTimeout then
				this:OnPointReached(false)
				return
			end

			-- Get Humanoid position and velocity
			this.CurrentHumanoidPosition = this.Humanoid.RootPart.Position + this.HumanoidOffsetFromPath
			this.CurrentHumanoidVelocity = this.Humanoid.RootPart.Velocity

			-- Check if it has reached some waypoints
			while this.Started and this:IsCurrentWaypointReached() do
				this:OnPointReached(true)
			end

			-- If still started, update actions
			if this.Started then
				-- Move action
				this.NextActionMoveDirection = this.CurrentWaypointPosition - this.CurrentHumanoidPosition
				if this.NextActionMoveDirection.Magnitude > ALMOST_ZERO then
					this.NextActionMoveDirection = this.NextActionMoveDirection.Unit
				else
					this.NextActionMoveDirection = ZERO_VECTOR3
				end
				-- Jump action
				if this.CurrentWaypointNeedsJump then
					this.NextActionJump = true
					this.CurrentWaypointNeedsJump = false	-- Request jump only once
				else
					this.NextActionJump = false
				end
			end
		end
	end

	function this:IsCurrentWaypointReached()
		local reached = false

		-- Check we do have a plane, if not, we consider the waypoint reached
		if this.CurrentWaypointPlaneNormal ~= ZERO_VECTOR3 then
			-- Compute distance of Humanoid from destination plane
			local dist = this.CurrentWaypointPlaneNormal:Dot(this.CurrentHumanoidPosition) - this.CurrentWaypointPlaneDistance
			-- Compute the component of the Humanoid velocity that is towards the plane
			local velocity = -this.CurrentWaypointPlaneNormal:Dot(this.CurrentHumanoidVelocity)
			-- Compute the threshold from the destination plane based on Humanoid velocity
			local threshold = math.max(1.0, 0.0625 * velocity)
			-- If we are less then threshold in front of the plane (between 0 and threshold) or if we are behing the plane (less then 0), we consider we reached it
			reached = dist < threshold
		else
			reached = true
		end

		if reached then
			this.CurrentWaypointPosition = nil
			this.CurrentWaypointPlaneNormal	= ZERO_VECTOR3
			this.CurrentWaypointPlaneDistance = 0
		end

		return reached
	end

	function this:OnPointReached(reached)

		if reached and not this.Cancelled then

			if FFlagUserClickToMoveFollowPathRefactor then
				-- First, destroyed the current displayed waypoint
				if this.setPointFunc then
					this.setPointFunc(this.CurrentPoint)
				end
			end

			local nextWaypointIdx = this.CurrentPoint + 1

			if nextWaypointIdx > #this.pointList then
				-- End of path reached
				if this.stopTraverseFunc then
					this.stopTraverseFunc()
				end
				this.Finished:Fire()
				this:Cleanup()
			else
				local currentWaypoint = this.pointList[this.CurrentPoint]
				local nextWaypoint = this.pointList[nextWaypointIdx]

				-- If airborne, only allow to keep moving
				-- if nextWaypoint.Action ~= Jump, or path mantains a direction
				-- Otherwise, wait until the humanoid gets to the ground
				local currentState = this.Humanoid:GetState()
				local isInAir = currentState == Enum.HumanoidStateType.FallingDown
					or currentState == Enum.HumanoidStateType.Freefall
					or currentState == Enum.HumanoidStateType.Jumping

				if isInAir then
					local shouldWaitForGround = nextWaypoint.Action == Enum.PathWaypointAction.Jump
					if not shouldWaitForGround and this.CurrentPoint > 1 then
						local prevWaypoint = this.pointList[this.CurrentPoint - 1]

						local prevDir = currentWaypoint.Position - prevWaypoint.Position
						local currDir = nextWaypoint.Position - currentWaypoint.Position

						local prevDirXZ = Vector2.new(prevDir.x, prevDir.z).Unit
						local currDirXZ = Vector2.new(currDir.x, currDir.z).Unit

						local THRESHOLD_COS = 0.996 -- ~cos(5 degrees)
						shouldWaitForGround = prevDirXZ:Dot(currDirXZ) < THRESHOLD_COS
					end

					if shouldWaitForGround then
						this.Humanoid.FreeFalling:Wait()

						-- Give time to the humanoid's state to change
						-- Otherwise, the jump flag in Humanoid
						-- will be reset by the state change
						wait(0.1)
					end
				end

				-- Move to the next point
				if FFlagUserNavigationClickToMoveSkipPassedWaypoints then
					if FFlagUserClickToMoveFollowPathRefactor then
						this:MoveToNextWayPoint(currentWaypoint, nextWaypoint, nextWaypointIdx)
					else
						-- First, check if we already passed the next point
						local nextWaypointAlreadyReached
						-- 1) Build plane (normal is from next waypoint towards current one
						-- (provided the two waypoints are not at the same location); location is at next waypoint)
						local planeNormal = currentWaypoint.Position - nextWaypoint.Position
						if planeNormal.Magnitude > ALMOST_ZERO then
							planeNormal	= planeNormal.Unit
							local planeDistance	= planeNormal:Dot(nextWaypoint.Position)
							-- 2) Find current Humanoid position
							local humanoidPosition = this.Humanoid.RootPart.Position - Vector3.new(
								0, 0.5 * this.Humanoid.RootPart.Size.y + this.Humanoid.HipHeight, 0)
							-- 3) Compute distance from plane
							local dist = planeNormal:Dot(humanoidPosition) - planeDistance
							-- 4) If we are less then a stud in front of the plane or if we are behing the plane, we consider we reached it
							nextWaypointAlreadyReached = dist < 1.0
						else
							-- Next waypoint is the same as current waypoint so we reached it as well
							nextWaypointAlreadyReached = true
						end

						-- Prepare for next point
						if not FFlagUserClickToMoveFollowPathRefactor then
							if this.setPointFunc then
								this.setPointFunc(nextWaypointIdx)
							end
						end
						this.CurrentPoint = nextWaypointIdx

						-- Either callback here right away if next waypoint is already passed
						-- Otherwise, ask the Humanoid to MoveTo
						if nextWaypointAlreadyReached then
							this:OnPointReached(true)
						else
							if nextWaypoint.Action == Enum.PathWaypointAction.Jump then
								this.Humanoid.Jump = true
							end
							this.Humanoid:MoveTo(nextWaypoint.Position)
						end
					end
				else
					if this.setPointFunc then
						this.setPointFunc(nextWaypointIdx)
					end
					if nextWaypoint.Action == Enum.PathWaypointAction.Jump then
						this.Humanoid.Jump = true
					end
					this.Humanoid:MoveTo(nextWaypoint.Position)

					this.CurrentPoint = nextWaypointIdx
				end
			end
		else
			this.PathFailed:Fire()
			this:Cleanup()
		end
	end

	function this:MoveToNextWayPoint(currentWaypoint, nextWaypoint, nextWaypointIdx)
		-- Build next destination plane
		-- (plane normal is perpendicular to the y plane and is from next waypoint towards current one (provided the two waypoints are not at the same location))
		-- (plane location is at next waypoint)
		this.CurrentWaypointPlaneNormal = currentWaypoint.Position - nextWaypoint.Position
		this.CurrentWaypointPlaneNormal = Vector3.new(this.CurrentWaypointPlaneNormal.X, 0, this.CurrentWaypointPlaneNormal.Z)
		if this.CurrentWaypointPlaneNormal.Magnitude > ALMOST_ZERO then
			this.CurrentWaypointPlaneNormal	= this.CurrentWaypointPlaneNormal.Unit
			this.CurrentWaypointPlaneDistance = this.CurrentWaypointPlaneNormal:Dot(nextWaypoint.Position)
		else
			-- Next waypoint is the same as current waypoint so no plane
			this.CurrentWaypointPlaneNormal	= ZERO_VECTOR3
			this.CurrentWaypointPlaneDistance = 0
		end

		-- Should we jump
		this.CurrentWaypointNeedsJump = nextWaypoint.Action == Enum.PathWaypointAction.Jump;

		-- Remember next waypoint position
		this.CurrentWaypointPosition = nextWaypoint.Position

		-- Move to next point
		this.CurrentPoint = nextWaypointIdx

		-- Finally reset Timeout
		this.Timeout = 0
	end

	function this:Start(overrideShowPath)
		if not this.AgentCanFollowPath then
			this.PathFailed:Fire()
			return
		end

		if this.Started then return end
		this.Started = true

		ClickToMoveDisplay.CancelFailureAnimation()

		if ShowPath then
			if overrideShowPath == nil or overrideShowPath then
				this.stopTraverseFunc, this.setPointFunc = ClickToMoveDisplay.CreatePathDisplay(this.pointList, this.OriginalTargetPoint)
			end
		end

		if #this.pointList > 0 then
			-- Determine the humanoid offset from the path's first point
			if FFlagUserClickToMoveFollowPathRefactor then
				-- Offset of the first waypoint from the path's origin point
				this.HumanoidOffsetFromPath = Vector3.new(0, this.pointList[1].Position.Y - this.OriginPoint.Y, 0)
			else
				-- Humanoid height above ground
				this.HumanoidOffsetFromPath = Vector3.new(0, -(0.5 * this.Humanoid.RootPart.Size.y + this.Humanoid.HipHeight), 0)
			end

			-- As well as its current position and velocity
			this.CurrentHumanoidPosition = this.Humanoid.RootPart.Position + this.HumanoidOffsetFromPath
			this.CurrentHumanoidVelocity = this.Humanoid.RootPart.Velocity

			-- Connect to events
			this.SeatedConn = this.Humanoid.Seated:Connect(function(isSeated, seat) this:OnPathInterrupted() end)
			this.DiedConn = this.Humanoid.Died:Connect(function() this:OnPathInterrupted() end)
			if not FFlagUserClickToMoveFollowPathRefactor then
				this.MoveToConn = this.Humanoid.MoveToFinished:Connect(function(reached) this:OnPointReached(reached) end)
			end
			if FFlagUserClickToMoveFollowPathRefactor then
				this.TeleportedConn = this.Humanoid.RootPart:GetPropertyChangedSignal("CFrame"):Connect(function() this:OnPathInterrupted() end)
			end

			-- Actually start
			this.CurrentPoint = 1 -- The first waypoint is always the start location. Skip it.
			this:OnPointReached(true) -- Move to first point
		else
			this.PathFailed:Fire()
			if this.stopTraverseFunc then
				this.stopTraverseFunc()
			end
		end
	end

	--We always raycast to the ground in the case that the user clicked a wall.
	local offsetPoint = this.TargetPoint + this.TargetSurfaceNormal*1.5
	local ray = Ray.new(offsetPoint, Vector3.new(0,-1,0)*50)
	local newHitPart, newHitPos = Workspace:FindPartOnRayWithIgnoreList(ray, getIgnoreList())
	if newHitPart then
		this.TargetPoint = newHitPos
	end
	this:ComputePath()

	return this
end
