--[[
	
	Inverse kinimatics solve.
	
	Functions.SolveIK(
		originCF,				<-- |REQ| 	Origin CFrame
		targetPos,				<-- |REQ| 	Target CFrame
		upperLength,			<-- |REQ| 	Upper length in studs
		lowerLength,			<-- |REQ| 	Lower length in studs
	)

--]]




return function (originCF, targetPos, l1, l2)	
	-- Build intial values for solving
	local localized = originCF:pointToObjectSpace(targetPos)
	local localizedUnit = localized.Unit
	local l3 = localized.Magnitude
	
	-- Build a "rolled" planeCF for a more natural arm look
	local axis = Vector3.new(0, 0, -1):Cross(localizedUnit)
	local angle = math.acos(-localizedUnit.Z)
	local planeCF = (originCF * CFrame.fromAxisAngle(axis, angle))
	
	-- Case: point is to close, unreachable
	-- Action: push back planeCF so the "hand" still reaches, angles fully compressed
	if l3 < (math.max(l2, l1) - math.min(l2, l1)) then
		return planeCF * CFrame.new(0, 0,  math.max(l2, l1) - math.min(l2, l1) - l3), -math.pi/2, math.pi
		
	-- Case: point is to far, unreachable
	-- Action: for forward planeCF so the "hand" still reaches, angles fully extended
	elseif l3 > (l1 + l2) then
		--return planeCF * CFrame.new(0, 0, l1 + l2 - l3), math.pi/2, 0
		return planeCF, math.pi/2, 0
		
	-- Case: point is reachable
	-- Action: planeCF is fine, solve the angles of the triangle
	else
		local a1 = -math.acos((-(l2 * l2) + (l1 * l1) + (l3 * l3)) / (2 * l1 * l3))
		local a2 = math.acos(((l2  * l2) - (l1 * l1) + (l3 * l3)) / (2 * l2 * l3))

		return planeCF, a1 + math.pi/2, a2 - a1
	end
end
