-- https://developer.roblox.com/en-us/articles/Inverse-Kinematics-for-Animation
-- edited slightly to enable solving a chain in parts

-- table sum function
local function summ(t,a,b)
	local s = 0;
	for i, value in ipairs(t) do
		if a and b then 
			if i>=a and i<=b then 
				s = s + value;
			end
		else 
			s = s + value;
		end
	end;
	return s;
end;
 
-- FABRIK chain class
local chain = {};
 
function chain.new(joints, bones, indexes)
	local self = setmetatable({}, {__index = chain});
 
	-- get the distances between the joints
	local lengths = {};
	for i = 1, #joints - 1 do
		lengths[i] = (joints[i] - joints[i+1]):len();
	end;	
	
	self.n = #joints;
	self.tolerance = 0.0001; -- heuristic method so this is our error margin
	self.bones = bones;
	self.joints = joints;
	self.indexes = indexes;
	self.lengths = lengths;
	self.origin = vec3(joints[1]);
	self.totallength = summ(lengths);
	
	-- rotation constraints (covered below)
	self.constrained = false;
	self.left = math.rad(89);
	self.right = math.rad(89);
	self.up = math.rad(89);
	self.down = math.rad(89);
	
	return self;
end;

function chain:backward(first,last)
	local last = last or self.n
	local first = first or 1
	-- backward reaching; set end effector as target
	self.joints[last] = self.target;
	for i = last - 1, first, -1 do
		local r = (self.joints[i+1] - self.joints[i]);
		local l = self.lengths[i] / r:len();
		-- find new joint position
		local pos = self.joints[i+1] * (1 - l) +  self.joints[i]* l
		self.joints[i] = pos;
	end;
end;
 
function chain:forward(first,last)
	local last = last or self.n
	local first = first or 1
	-- forward reaching; set root at initial position
	self.joints[first] = self.origin;
	for i = 1, last - 1 do
		local r = (self.joints[i+1] - self.joints[i]);
		local l = self.lengths[i] / r:len();
		-- find new joint position
		local pos = self.joints[i] * (1 - l) +  self.joints[i+1]* l
		self.joints[i+1] = pos;
	end;
end;
 
function chain:solve(first,last)
	local total = (first) and summ(self.lengths,first,last) or self.totallength 
	local last = last or self.n
	local first = first or 1
	local distance = (self.joints[first] - self.target):len();

	if distance > total then
		-- target is out of reach
		for i = first, last - 1 do
			local r = (self.target - self.joints[i]):len();
			local l = self.lengths[i] / r;
			-- find new joint position
			self.joints[i+1] = self.joints[i] * (1 - l) +  self.target* l
		end;
	else
		-- target is in reach
		local bcount = 0;
		local dif = (self.joints[last] - self.target):len();
		while dif > self.tolerance do -- check if within error margin
			self:backward(first,last);
			self:forward(first,last);
			dif = (self.joints[last] - self.target):len();
			-- break if it's taking too long so the game doesn't freeze
			bcount = bcount + 1;
			if bcount > 10 then break; end;
		end; 
	end;
end;

function chain:constrain(calc, line, cf)
	local scalar = calc:dot(line) / line:len();
	local proj = scalar * line.unit;
	
	-- get axis that are closest
	local ups = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Top)), cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Bottom))};
	local rights = {cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Right)),  cf:vectorToWorldSpace(Vector3.FromNormalId(Enum.NormalId.Left))};
	table.sort(ups, function(a, b) return (a - calc):len() < (b - calc):len() end);
	table.sort(rights, function(a, b) return (a - calc):len() < (b - calc):len() end);
	
	local upvec = ups[1];
	local rightvec = rights[1];
 
	-- get the vector from the projection to the calculated vector
	local adjust = calc - proj;
	if scalar < 0 then
		-- if we're below the cone flip the projection vector
		proj = -proj;
	end;
	
	-- get the 2D components
	local xaspect = adjust:dot(rightvec);
	local yaspect = adjust:dot(upvec);
	
	-- get the cross section of the cone
	local left = -(proj:len() * math.tan(self.left));
	local right = proj:len() * math.tan(self.right);
	local up = proj:len() * math.tan(self.up);
	local down = -(proj:len() * math.tan(self.down));
	
	-- find the quadrant
	local xbound = xaspect >= 0 and right or left;
	local ybound = yaspect >= 0 and up or down;
	
	local f = calc;
	-- check if in 2D point lies in the ellipse 
	local ellipse = xaspect^2/xbound^2 + yaspect^2/ybound^2;
	local inbounds = ellipse <= 1 and scalar >= 0;
	
	if not inbounds then
		-- get the angle of our out of ellipse point
		local a = math.atan2(yaspect, xaspect);
		-- find nearest point
		local x = xbound * math.cos(a);
		local y = ybound * math.sin(a);
		-- convert back to 3D
		f = (proj + rightvec * x + upvec * y).unit * calc:len();
	end;
	
	-- return our final vector
	return f;
end;

return chain 

-- local ik = require(game.Workspace.FABRIK);
 
-- local start = game.Workspace.Start;
-- local subb = game.Workspace.SubBase;
-- local finish1 = game.Workspace.Finish1;
-- local finish2 = game.Workspace.Finish2;
 
-- -- build our chains
-- local A = buildChain(start.Position, subb.Position, 5);
-- local chainA = ik.new(A, subb.Position);
-- local A1 = buildChain(subb.Position, finish1.Position, 5);
-- local chainA1 = ik.new(A1, finish1.Position);
-- local A2 = buildChain(subb.Position, finish1.Position, 5);
-- local chainA2 = ik.new(A2, finish2.Position);
 
-- function update()
-- 	-- update the targets
-- 	chainA.origin = CFrame.new(start.Position);
-- 	chainA.joints[1] = start.Position;
-- 	chainA1.target = finish1.Position;
-- 	chainA2.target = finish2.Position;
	
-- 	-- work backwards to the subb
-- 	chainA1:backward();
-- 	chainA2:backward();
	
-- 	-- get the centroid, set as next part of chain's target and solve 
-- 	-- (we're going backwards, then forwards by using solve for chainA)
-- 	local centroid = (chainA1.joints[1] + chainA2.joints[2])/2;
-- 	chainA.target = centroid;
-- 	chainA:solve();
	
-- 	-- set origins to end of chainA
-- 	chainA1.origin = CFrame.new(chainA.joints[chainA.n]);
-- 	chainA2.origin = CFrame.new(chainA.joints[chainA.n]);
	
-- 	-- now we're working forwards for the things we previously went backwards on
-- 	chainA1:forward();
-- 	chainA2:forward();
	
-- 	-- from here you'd draw the chain...
-- end