Teleport script not working

Heya fellas! So I made this teleport script, it either teleports to the mouse target or if you are locked onto a player, it teleports onto them. Now, if you try to teleport and exceed the TPRange, it will instead teleport to the max possible range but in the direction the player/mouse is in.
The problem is that it is really messy, while it does take mana and does not have any errors, it sadly teleports to weird places or not at all.

local Players = game:GetService("Players")
local ReplicatedStorage = game:GetService("ReplicatedStorage")
local ServerScriptService = game:GetService("ServerScriptService")
local ServerStorage = game:GetService("ServerStorage")

local Modules = ServerScriptService:WaitForChild("Modules")

local Remotes = ReplicatedStorage:WaitForChild("Remotes")
local TeleportRemote = Remotes.RequestTeleport
local Utils = require(Modules:WaitForChild("Utils"))
local CharacterAbilities = require(Modules:WaitForChild("CharacterAbilities"))

local TakeMana = require(Modules.StatModules:WaitForChild("TakeMana"))

TeleportRemote.OnServerEvent:Connect(function(plr, mouse)
	local Data = Utils.GetData(plr)
	if not Data then return plr:Kick(Utils.Error("No data.")) end
	local PlayerChar = Data.Character.Value
	if PlayerChar ~= "None" then
		local plrchar = plr.Character
		local Head = plrchar.Head
		local TPRange = CharacterAbilities[PlayerChar].TPRange
		local Magnitude = (Head.Position - mouse).Magnitude
		local HumanoidRootPart = plrchar.HumanoidRootPart
		local Mana = Data.Mana
		local TPManaCost = CharacterAbilities[PlayerChar].TPManaCost
		if Data.lockedOnto.Value ~= "None" then
			local playerlocked = Data.lockedOnto.Value
			local Players = game:GetService("Players")
			local playertar = Players:FindFirstChild(playerlocked)
			local targetchar = playertar.Character
			local targethead = targetchar.Head
			local Magnitude2 = (Head.Position - targethead.Position).Magnitude
			if TPRange > Magnitude2 then
				if Data.Mana.Value >= TPManaCost then
					local OFFSET = 4
					targetchar:WaitForChild("HumanoidRootPart").CFrame = plrchar:WaitForChild("HumanoidRootPart").CFrame * CFrame.new(0, 0, -OFFSET)
					local oldmanaval = Data.Mana.Value
					local resultmana = oldmanaval - TPManaCost
					Data.Mana.Value = resultmana
				else
				end
			end
		elseif TPRange > Magnitude then
			if Data.Mana.Value >= TPManaCost then
				HumanoidRootPart.Position = mouse
				local oldmanaval = Data.Mana.Value
				local resultmana = oldmanaval - TPManaCost
				Data.Mana.Value = resultmana
			else
			end
		elseif Magnitude > 0 then
			if Data.Mana.Value >= TPManaCost then
				HumanoidRootPart.Position = (mouse - Head.Position).Unit * TPRange + Head.Position
				local oldmanaval = Data.Mana.Value
				local resultmana = oldmanaval - TPManaCost
				Data.Mana.Value = resultmana
			end
		end
	end
end)