-----------------------------------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.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

	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

	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 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
			this.PathComputing = false
		end
	end

	function this:IsValidPath()
		this:ComputePath()
		return this.PathComputed and this.AgentCanFollowPath
	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

		this.OriginPoint = this.Humanoid.RootPart.CFrame.p

		this.pathResult:ComputeAsync(this.OriginPoint, this.TargetPoint)
		this.pointList = this.pathResult:GetWaypoints()
		if #this.pointList > 0 then
			this.HumanoidOffsetFromPath = this.pointList[1].Position - this.OriginPoint
		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
			-- First, destroyed the current displayed waypoint
			if this.setPointFunc then
				this.setPointFunc(this.CurrentPoint)
			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
					this:MoveToNextWayPoint(currentWaypoint, nextWaypoint, nextWaypointIdx)
				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
			-- 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)

			-- 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)
			this.TeleportedConn = this.Humanoid.RootPart:GetPropertyChangedSignal("CFrame"):Connect(function() this:OnPathInterrupted() 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