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

local function CreateDestinationIndicator(pos)
	local destinationGlobe = Create'Part'
	{
		Name = 'PathGlobe';
		TopSurface = 'Smooth';
		BottomSurface = 'Smooth';
		Shape = 'Ball';
		CanCollide = false;
		Size = Vector3_new(2,2,2);
		BrickColor = BrickColor.new('Institutional white');
		Transparency = 0;
		Anchored = true;
		CFrame = CFrame.new(pos);
	}
	return destinationGlobe
end

local function Pather(character, point)
	local this = {}

	this.Cancelled = false
	this.Started = false

	this.Finished = Signal.Create()
	this.PathFailed = Signal.Create()
	this.PathStarted = Signal.Create()

	this.PathComputed = false

	function this:YieldUntilPointReached(character, point, timeout)
		timeout = timeout or 10000000

		local humanoid = findPlayerHumanoid(Player)
		local torso = humanoid and humanoid.Torso
		local start = tick()
		local lastMoveTo = start
		while torso and tick() - start < timeout and this.Cancelled == false do
			local diffVector = (point - torso.CFrame.p)
			local xzMagnitude = (diffVector * XZ_VECTOR3).magnitude
			if xzMagnitude < 6 then
				-- Jump if the path is telling is to go upwards
				if diffVector.Y >= 2.2 then
					humanoid.Jump = true
				end
			end
			-- The hard-coded number 2 here is from the engine's MoveTo implementation
			if xzMagnitude < 2 then
				return true
			end
			-- Keep on issuing the move command because it will automatically quit every so often.
			if tick() - lastMoveTo > 1.5 then
				humanoid:MoveTo(point)
				lastMoveTo = tick()
			end
			CharacterControl:UpdateTorso(point)
			task.wait()
		end
		return false
	end

	function this:Cancel()
		this.Cancelled = true
		local humanoid = findPlayerHumanoid(Player)
		local torso = humanoid and humanoid.Torso
		if humanoid and torso then
			humanoid:MoveTo(torso.CFrame.p)
		end
	end

	function this:CheckOcclusion(point1, point2, character, torsoRadius)
		local humanoid = findPlayerHumanoid(Player)
		local torso = humanoid and humanoid.Torso
		if torsoRadius == nil then
			torsoRadius = torso and Vector3_new(torso.Size.X/2,0,torso.Size.Z/2) or XZ_VECTOR3
		end

		local diffVector = point2 - point1
		local directionVector = diffVector.unit

		local rightVector = Y_VECTOR3:Cross(directionVector) * torsoRadius

		local rightPart, _ = Utility.Raycast(Ray.new(point1 + rightVector, diffVector + rightVector), true, {character})
		local hitPart, _ = Utility.Raycast(Ray.new(point1, diffVector), true, {character})
		local leftPart, _ = Utility.Raycast(Ray.new(point1 - rightVector, diffVector - rightVector), true, {character})

		if rightPart or hitPart or leftPart then
			return false
		end

		-- Make sure we have somewhere to stand on
		local midPt = (point2 + point1) / 2
		local studsBetweenSamples = 2
		for i = 1, math_floor(diffVector.magnitude/studsBetweenSamples) do
			local downPart, _ = Utility.Raycast(Ray.new(point1 + directionVector * i * studsBetweenSamples, Vector3_new(0,-7,0)), true, {character})
			if not downPart then
				return false
			end
		end

		return true
	end

	function this:SmoothPoints(pathToSmooth)
		local result = {}

		local humanoid = findPlayerHumanoid(Player)
		local torso = humanoid and humanoid.Torso
		for i = 1, #pathToSmooth do
			table.insert(result, pathToSmooth[i])
		end

		-- Backwards for safe-deletion
		for i = #result - 1, 1, -1 do
			if i + 1 <= #result then

				local nextPoint = result[i+1]
				local thisPoint = result[i]

				local lastPoint = result[i-1]
				if lastPoint == nil then
					lastPoint = torso and Vector3_new(torso.CFrame.p.X, thisPoint.Y, torso.CFrame.p.Z)
				end

				if lastPoint and Utility.FuzzyEquals(thisPoint.Y, lastPoint.Y) and Utility.FuzzyEquals(thisPoint.Y, nextPoint.Y) then
					if this:CheckOcclusion(lastPoint, nextPoint, character) then
						table.remove(result, i)
						-- Move i back one to recursively-smooth
						i = i + 1
					end
				end
			end
		end

		return result
	end

	function this:CheckNeighboringCells(character)
		local pathablePoints = {}
		local humanoid = findPlayerHumanoid(Player)
		local torso = character and humanoid and humanoid.Torso
		if torso then
			local torsoCFrame = torso.CFrame
			local torsoPos = torsoCFrame.p
			-- Minus and plus 2 is so we can get it into the cell-corner space and then translate it back into cell-center space
			local roundedPos = Vector3_new(Utility.Round(torsoPos.X-2,4)+2, Utility.Round(torsoPos.Y-2,4)+2, Utility.Round(torsoPos.Z-2,4)+2)
			local neighboringCells = {}
			for x = -4, 4, 8 do
				for z = -4, 4, 8 do
					table.insert(neighboringCells, roundedPos + Vector3_new(x,0,z))
				end
			end
			for _, testPoint in pairs(neighboringCells) do
				local pathable = this:CheckOcclusion(roundedPos, testPoint, character, ZERO_VECTOR3)
				if pathable then
					table.insert(pathablePoints, testPoint)
				end
			end
		end
		return pathablePoints
	end

	function this:ComputeDirectPath()
		local humanoid = findPlayerHumanoid(Player)
		local torso = humanoid and humanoid.Torso
		if torso then
			local startPt = torso.CFrame.p
			local finishPt = point
			if (finishPt - startPt).magnitude < 150 then
				-- move back the destination by 2 studs or otherwise the pather will collide with the object we are trying to reach
				finishPt = finishPt - (finishPt - startPt).unit * 2
				if this:CheckOcclusion(startPt, finishPt, character, ZERO_VECTOR3) then
					local pathResult = {}
					pathResult.Status = Enum.PathStatus.Success
					function pathResult:GetPointCoordinates()
						return {finishPt}
					end
					return pathResult
				end
			end
		end
	end

	local function AllAxisInThreshhold(targetPt, otherPt, threshold)
		return math_abs(targetPt.X - otherPt.X) <= threshold and
			math_abs(targetPt.Y - otherPt.Y) <= threshold and
			math_abs(targetPt.Z - otherPt.Z) <= threshold
	end

	function this:ComputePath()
		local smoothed = false
		local humanoid = findPlayerHumanoid(Player)
		local torso = humanoid and humanoid.Torso
		if torso then
			if this.PathComputed then return end
			this.PathComputed = true
			-- Will yield the script since it is an Async script (start, finish, maxDistance)
			-- Try to use the smooth function, but it may not exist yet :(
			local success = pcall(function()
				-- 3 is height from torso cframe to ground
				this.pathResult = PathfindingService:ComputeSmoothPathAsync(torso.CFrame.p - Vector3_new(0,3,0), point, 400)
				smoothed = true
			end)
			if not success then
				-- 3 is height from torso cframe to ground
				this.pathResult = PathfindingService:ComputeRawPathAsync(torso.CFrame.p - Vector3_new(0,3,0), point, 400)
				smoothed = false
			end
			this.pointList = this.pathResult and this.pathResult:GetPointCoordinates()
			local pathFound = false
			if this.pathResult.Status == Enum.PathStatus.FailFinishNotEmpty then
				-- Lets try again with a slightly set back start point; it is ok to do this again so the FailFinishNotEmpty uses little computation
				local diffVector = point - workspace.CurrentCamera.CoordinateFrame.p
				if diffVector.magnitude > 2 then
					local setBackPoint = point - (diffVector).unit * 2.1
					local success = pcall(function()
						this.pathResult = PathfindingService:ComputeSmoothPathAsync(torso.CFrame.p, setBackPoint, 400)
						smoothed = true
					end)
					if not success then
						this.pathResult = PathfindingService:ComputeRawPathAsync(torso.CFrame.p, setBackPoint, 400)
						smoothed = false
					end
					this.pointList = this.pathResult and this.pathResult:GetPointCoordinates()
					pathFound = true
				end
			end
			if this.pathResult.Status == Enum.PathStatus.ClosestNoPath and #this.pointList >= 1 and pathFound == false then
				local otherPt = this.pointList[#this.pointList]
				if AllAxisInThreshhold(point, otherPt, 4) and (torso.CFrame.p - point).magnitude > (otherPt - point).magnitude then
					local pathResult = {}
					pathResult.Status = Enum.PathStatus.Success
					function pathResult:GetPointCoordinates()
						return {this.pointList}
					end
					this.pathResult = pathResult
					pathFound = true
				end
			end
			if (this.pathResult.Status == Enum.PathStatus.FailStartNotEmpty or this.pathResult.Status == Enum.PathStatus.ClosestNoPath) and pathFound == false then
				local pathablePoints = this:CheckNeighboringCells(character)
				for _, otherStart in pairs(pathablePoints) do
					local pathResult;
					local success = pcall(function()
						pathResult = PathfindingService:ComputeSmoothPathAsync(otherStart, point, 400)
						smoothed = true
					end)
					if not success then
						pathResult = PathfindingService:ComputeRawPathAsync(otherStart, point, 400)
						smoothed = false
					end
					if pathResult and pathResult.Status == Enum.PathStatus.Success then
						this.pathResult = pathResult
						if this.pathResult then
							this.pointList = this.pathResult:GetPointCoordinates()
							table.insert(this.pointList, 1, otherStart)
						end
						break
					end
				end
			end
			if DirectPathEnabled then
				if this.pathResult.Status ~= Enum.PathStatus.Success then
					local directPathResult = this:ComputeDirectPath()
					if directPathResult and directPathResult.Status == Enum.PathStatus.Success then
						this.pathResult = directPathResult
						this.pointList = directPathResult:GetPointCoordinates()
					end
				end
			end
		end
		return smoothed
	end

	function this:IsValidPath()
		this:ComputePath()
		local pathStatus = this.pathResult.Status
		return pathStatus == Enum.PathStatus.Success
	end

	function this:GetPathStatus()
		this:ComputePath()
		return this.pathResult.Status
	end

	function this:Start()
		if CurrentSeatPart then
			return
		end
		spawn(function()
			local humanoid = findPlayerHumanoid(Player)
			--humanoid.AutoRotate = false
			local torso = humanoid and humanoid.Torso
			if torso then
				if this.Started then return end
				this.Started = true
				-- Will yield the script since it is an Async function script (start, finish, maxDistance)
				local smoothed = this:ComputePath()
				if this:IsValidPath() then
					this.PathStarted:fire()
					-- smooth out zig-zaggy paths
					local smoothPath = smoothed and this.pointList or this:SmoothPoints(this.pointList)
					for i, point in pairs(smoothPath) do
						if humanoid then
							if this.Cancelled then
								return
							end

							local wayPoint = nil
							if SHOW_PATH then
								wayPoint = CreateDestinationIndicator(point)
								wayPoint.BrickColor = BrickColor.new("New Yeller")
								wayPoint.Parent = workspace
								print(wayPoint.CFrame.p)
							end

							humanoid:MoveTo(point)

							local distance = ((torso.CFrame.p - point) * XZ_VECTOR3).magnitude
							local approxTime = 10
							if math_abs(humanoid.WalkSpeed) > 0 then
								approxTime = distance / math_abs(humanoid.WalkSpeed)
							end

							local yielding = true

							if i == 1 then
								--local rotatedCFrame = CameraModule:LookAtPreserveHeight(point)
								if CameraModule then
									local rotatedCFrame = CameraModule:LookAtPreserveHeight(smoothPath[#smoothPath])
									local finishedSignal, duration = CameraModule:TweenCameraLook(rotatedCFrame)
								end
								--CharacterControl:SetTorsoLookPoint(point)
							end
							---[[
							if (humanoid.Torso.CFrame.p - point).magnitude > 9 then
								spawn(function()
									while yielding and this.Cancelled == false do
										if CameraModule then
											local look = CameraModule:GetCameraLook()
											local squashedLook = (look * XZ_VECTOR3).unit
											local direction = ((point - CameraModule.cframe.p) * XZ_VECTOR3).unit

											local theta = math_deg(math_acos(squashedLook:Dot(direction)))

											if tick() - Utility.GetLastInput() > 2 and theta > (workspace.CurrentCamera.FieldOfView / 2) then
												local rotatedCFrame = CameraModule:LookAtPreserveHeight(point)
												local finishedSignal, duration = CameraModule:TweenCameraLook(rotatedCFrame)
												--return
											end
										end
										task.wait(0.1)
									end
								end)
							end
							--]]
							local didReach = this:YieldUntilPointReached(character, point, approxTime * 3 + 1)

							yielding = false

							if SHOW_PATH then
								wayPoint:Destroy()
							end

							if not didReach then
								this.PathFailed:fire()
								return
							end
						end
					end

					this.Finished:fire()
					return
				end
			end
			this.PathFailed:fire()
		end)
	end

	return this
end