-- Smart Circle mode needs the intersection of 2 rays that are known to be in the same plane
-- because they are generated from cross products with a common vector. This function is computing
-- that intersection, but it's actually the general solution for the point halfway between where
-- two skew lines come nearest to each other, which is more forgiving.
local function RayIntersection(p0, v0, p1, v1)
	local v2 = v0:Cross(v1)
	local d1 = p1.x - p0.x
	local d2 = p1.y - p0.y
	local d3 = p1.z - p0.z
	local denom = Det3x3(v0.x,-v1.x,v2.x,v0.y,-v1.y,v2.y,v0.z,-v1.z,v2.z)
	
	if (denom == 0) then
		return ZERO_VECTOR3 -- No solution (rays are parallel)
	end
	
	local t0 = Det3x3(d1,-v1.x,v2.x,d2,-v1.y,v2.y,d3,-v1.z,v2.z) / denom
	local t1 = Det3x3(v0.x,d1,v2.x,v0.y,d2,v2.y,v0.z,d3,v2.z) / denom
	local s0 = p0 + t0 * v0
	local s1 = p1 + t1 * v1
	local s = s0 + 0.5 * ( s1 - s0 )
	
	-- 0.25 studs is a threshold for deciding if the rays are
	-- close enough to be considered intersecting, found through testing 
	if (s1-s0).Magnitude < 0.25 then
		return s
	else
		return ZERO_VECTOR3
	end
end

local function SmartCircleBehavior(castPoints)
	local torsoUp = TorsoPart.CFrame.upVector.unit
	local torsoRight = TorsoPart.CFrame.rightVector.unit
	
	-- SMART_CIRCLE mode includes rays to head and 5 to the torso.
	-- Hands, arms, legs and feet are not included since they
	-- are not canCollide and can therefore go inside of parts
	castPoints[#castPoints + 1] = TorsoPart.CFrame.p
	castPoints[#castPoints + 1] = TorsoPart.CFrame.p + torsoUp
	castPoints[#castPoints + 1] = TorsoPart.CFrame.p - torsoUp
	castPoints[#castPoints + 1] = TorsoPart.CFrame.p + torsoRight
	castPoints[#castPoints + 1] = TorsoPart.CFrame.p - torsoRight
	if HeadPart then
		castPoints[#castPoints + 1] = HeadPart.CFrame.p
	end
	
	local cameraOrientation = Camera.CFrame - Camera.CFrame.p
	local torsoPoint = Vector3_new(0,0.5,0) + (TorsoPart and TorsoPart.Position or HumanoidRootPart.Position)
	local radius = 2.5
	
	-- This loop first calculates points in a circle of radius 2.5 around the torso of the character, in the
	-- plane orthogonal to the camera's lookVector. Each point is then raycast to, to determine if it is within
	-- the free space surrounding the player (not inside anything). Two iterations are done to adjust points that
	-- are inside parts, to try to move them to valid locations that are still on their camera ray, so that the
	-- circle remains circular from the camera's perspective, but does not cast rays into walls or parts that are
	-- behind, below or beside the character and not really obstructing view of the character. This minimizes
	-- the undesirable situation where the character walks up to an exterior wall and it is made invisible even
	-- though it is behind the character.
	for i = 1, SMART_CIRCLE_CASTS do
		local angle = SMART_CIRCLE_INCREMENT * i - 0.5 * math.pi
		local offset = radius * Vector3_new(math_cos(angle), math_sin(angle), 0)
		local circlePoint = torsoPoint + cameraOrientation * offset		
		 
		-- Vector from camera to point on the circle being tested		
		local vp = circlePoint - Camera.CFrame.p
		
		local ray = Ray.new(torsoPoint, circlePoint - torsoPoint)
		local hit, hp, hitNormal = game.Workspace:FindPartOnRayWithIgnoreList(ray, {Character}, false, false )
		local castPoint = circlePoint
				
		if hit then
			local hprime = hp + 0.1 * hitNormal.unit -- Slightly offset hit point from the hit surface
			local v0 = hprime - torsoPoint -- Vector from torso to offset hit point
			local d0 = v0.magnitude
			
			local perp = (v0:Cross(vp)).unit

			-- Vector from the offset hit point, along the hit surface
			local v1 = (perp:Cross(hitNormal)).unit
			
			-- Vector from camera to offset hit
			local vprime = (hprime - Camera.CFrame.p).unit
			
			-- This dot product checks to see if the vector along the hit surface would hit the correct
			-- side of the invisicam cone, or if it would cross the camera look vector and hit the wrong side
			if ( v0.unit:Dot(-v1) < v0.unit:Dot(vprime)) then
				castPoint = RayIntersection(hprime, v1, circlePoint, vp)
				
				if castPoint.Magnitude > 0 then
					local ray = Ray.new(hprime, castPoint - hprime)
					local hit, hitPoint, hitNormal = game.Workspace:FindPartOnRayWithIgnoreList(ray, {Character}, false, false )
					
					if hit then
						local hprime2 = hitPoint + 0.1 * hitNormal.unit
						castPoint = hprime2
					end
				else
					castPoint = hprime
				end
			else
				castPoint = hprime
			end
			
			local ray = Ray.new(torsoPoint, (castPoint - torsoPoint))
			local hit, hitPoint, hitNormal = game.Workspace:FindPartOnRayWithIgnoreList(ray, {Character}, false, false )
	
			if hit then
				local castPoint2 = hitPoint - 0.1 * (castPoint - torsoPoint).unit
				castPoint = castPoint2	
			end
		end
		
		castPoints[#castPoints + 1] = castPoint
	end
end

local function CheckTorsoReference()
	if Character then
		TorsoPart = Character:FindFirstChild("Torso")
		if not TorsoPart then
			TorsoPart = Character:FindFirstChild("UpperTorso")
			if not TorsoPart then
				TorsoPart = Character:FindFirstChild("HumanoidRootPart")
			end
		end
		
		HeadPart = Character:FindFirstChild("Head")
	end
end

local function OnCharacterAdded(character)
	if childAddedConn then
		childAddedConn:disconnect()
		childAddedConn = nil
	end
	if childRemovedConn then
		childRemovedConn:disconnect()
		childRemovedConn = nil
	end

	Character = character
	
	TrackedLimbs = {}
	local function childAdded(child)
		if child:IsA('BasePart') then
			if LIMB_TRACKING_SET[child.Name] then
				TrackedLimbs[child] = true
			end

			if (child.Name == 'Torso' or child.Name == 'UpperTorso') then
				TorsoPart = child
			end

			if (child.Name == 'Head') then
				HeadPart = child
			end			
		end
	end
	
	local function childRemoved(child)
		TrackedLimbs[child] = nil
		
		-- If removed/replaced part is 'Torso' or 'UpperTorso' double check that we still have a TorsoPart to use
		CheckTorsoReference()
	end	
	
	childAddedConn = character.ChildAdded:connect(childAdded)
	childRemovedConn = character.ChildRemoved:connect(childRemoved)
	
	for _, child in pairs(Character:GetChildren()) do
		childAdded(child)
	end
end

local function OnCurrentCameraChanged()
	local newCamera = workspace.CurrentCamera
	if newCamera then
		Camera = newCamera
	end
end