Custom Movement Code Not Working


AddCSLuaFile( "cl_init.lua" )
AddCSLuaFile( "shared.lua" )
include( 'shared.lua' )

function ENT:Initialize()
	self.Entity:SetModel( "models/Humans/Group01/male_07.mdl" )
	
	self.Entity:PhysicsInit( SOLID_VPHYSICS )
	self.Entity:SetMoveType( MOVETYPE_NONE )
	self.Entity:SetSolid( SOLID_VPHYSICS )
	self.Entity.PosToMoveTo = self.Entity:GetPos()
	self.ShouldMove = false
end

function ENT:Move( pos )
	self.ShouldMove = true
	self.PosToMoveTo = pos
end

function ENT:StopMoving()
	self.ShouldMove = false
	self.PosToMoveTo = self:GetPos()
end

local VmPos = Vector(0,0,0)
local WantedPos = Vector(0,0,0)
local SetPos = Vector(0,0,0)
function ENT:HandleMoving( pos, ang )
	if self.ShouldMove then
		SetPos = self.PosToMoveTo
	elseif self.ShouldMove and self.PosToMoveTo == self:GetPos() || self.ShouldMove and !util.IsInWorld(self.PosToMoveTo) || self.ShouldMove and !self:IsInWorld() then
		self:StopMoving()
		return
	end
	
	WantedPos = LerpVector( FrameTime() * 15, WantedPos, SetPos)
	VmPos.x = WantedPos.x
	VmPos.y = WantedPos.y
	VmPos.z = WantedPos.z
	
	pos = pos + VmPos.x * ang:Right()
	pos = pos + VmPos.y * ang:Forward()
	pos = pos + VmPos.z * ang:Up()

	self:SetPos( pos )
end

function ENT:Think()
	self:HandleMoving( self:GetPos(), self:GetAngles() )
end

The entity just keeps on moving upwards until they get into the skybox. What am I doing wrong? I want the entity to stay still unless it has a position to move to(that isn’t it’s own position) or it is blocked by something. What am I doing wrong?