local chain = {}
local maths = {}
local joint = {}
local rig = {}

do
    function chain.new(joints, target)
        local function sum(t)
            local s = 0;
            for _, value in ipairs(t) do
                s = s + value;
            end;
            return s;
        end;

        local self = setmetatable({}, {__index = chain});

        local lengths = {};
        for i = 1, #joints - 1 do
            lengths[i] = (joints[i] - joints[i+1]).magnitude;
        end;

        self.n = #joints;
        self.tolerance = 0.001;
        self.target = target;
        self.joints = joints;
        self.lengths = lengths;
        self.origin = CFrame.new(joints[1]);
        self.totallength = sum(lengths);

        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:constrain(calc, line, cf)
        local scalar = calc:Dot(line) / line.magnitude;
        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).magnitude < (b - calc).magnitude end);
        table.sort(rights, function(a, b) return (a - calc).magnitude < (b - calc).magnitude 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.magnitude * math.tan(self.left));
        local right = proj.magnitude * math.tan(self.right);
        local up = proj.magnitude * math.tan(self.up);
        local down = -(proj.magnitude * 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.magnitude;
        end;

        -- return our final vector
        return f;
    end;

    function chain:backward()
        -- backward reaching; set end effector as target
        self.joints[self.n] = self.target;
        for i = self.n - 1, 1, -1 do
            local r = (self.joints[i+1] - self.joints[i]);
            local l = self.lengths[i] / r.magnitude;
            -- find new joint position
            local pos = (1 - l) * self.joints[i+1] + l * self.joints[i];
            self.joints[i] = pos;
        end;
    end;

    function chain:forward()
        -- forward reaching; set root at initial position
        self.joints[1] = self.origin.p;
        local coneVec = (self.joints[2] - self.joints[1]).unit;
        for i = 1, self.n - 1 do
            local r = (self.joints[i+1] - self.joints[i]);
            local l = self.lengths[i] / r.magnitude;
            -- setup matrix
            local cf = CFrame.new(self.joints[i], self.joints[i] + coneVec);
            -- find new joint position
            local pos = (1 - l) * self.joints[i] + l * self.joints[i+1];
            local t = self:constrain(pos - self.joints[i], coneVec, cf);
            self.joints[i+1] = self.constrained and self.joints[i] + t or pos;
            coneVec = self.joints[i+1] - self.joints[i];
        end;
    end;

    function chain:solve()
        local distance = (self.joints[1] - self.target).magnitude;
        if distance > self.totallength then
            -- target is out of reach
            for i = 1, self.n - 1 do
                local r = (self.target - self.joints[i]).magnitude;
                local l = self.lengths[i] / r;
                -- find new joint position
                self.joints[i+1] = (1 - l) * self.joints[i] + l * self.target;
            end;
        else
            -- target is in reach
            local bcount = 0;
            local dif = (self.joints[self.n] - self.target).magnitude;
            while dif > self.tolerance do
                self:backward();
                self:forward();
                dif = (self.joints[self.n] - self.target).magnitude;
                -- 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;
end


do
    function maths.cosine(a,b,c) --a is left of the angle, b is right of the angle, c is opposite to the angle
        return math.acos((a*a+b*b-c*c)/(2*a*b))
    end

    function maths.projectvector(a, b)
        return a - a:Dot(b.unit)*b
    end

    function maths.vectortoangle(v0,v1,plane)
        v0,v1 = maths.projectvector(v0.unit, plane).unit, maths.projectvector(v1.unit, plane).unit
        local angle = math.acos(v0:Dot(v1))
        local cross = v0:Cross(v1)
        if plane:Dot(cross) < 0 then
            angle = -angle
        end
        return angle
    end
end


do
    function joint.new(parent,c0,c1,p0,p1,v0)
        local self = setmetatable({}, {__index = joint})

        self.weld = Instance.new("ManualWeld", parent)
        self.weld.Part0 = p0
        self.weld.Part1 = p1
        self.weld.C0 = c0
        self.weld.C1 = c1

        self.c0 = c0
        self.c1 = c1
        self.p0 = p0
        self.p1 = p0
        self.velocity = v0 or Vector3.new(1,1,1)

        return self
    end

    function joint:setproperty(property, value)
        if self.weld[property] then
            self.weld[property] = value
        end
    end
end


do
    function rig.new()
        local self = setmetatable({}, {__index = rig})

        self.character = game.Players.LocalPlayer.Character --// Set to instantiated character
        self.head = Instance.new("Part",self.character)
        self.head.Size = Vector3.new(.2,.2,.2)
        self.head.Transparency = 1
        self.neck = joint.new(self.head, CFrame.new(0,-1.5,0), CFrame.new(), self.head, self.character.HumanoidRootPart)
        self.pointTo = Instance.new("Part",self.character)
        self.pointTo.Size = Vector3.new(.2,.2,.2)
        self.pointJoint = joint.new(self.pointTo, CFrame.new(0,0,2), CFrame.new(), self.pointTo, self.head)
        local step = 0
        game:GetService("RunService").RenderStepped:connect(function()
            step = step + 1
            if step >= 60 then
                step = 0
            end

            self.pointJoint.weld.C1 = CFrame.new(0,-step/60,0)
        end)

        return self
    end

    function rig:createarms()
        self.rightarm = game.ReplicatedStorage.Arm:Clone()
        self.rightarm.Parent = self.character
        self.rightelbow = joint.new(self.rightarm.Bicep, CFrame.new(0,-.7,0), CFrame.new(0, .8, 0), self.rightarm.Bicep, self.rightarm.Forearm)
        self.rightshoulder = joint.new(self.head, CFrame.new(.4,-.7,0) * CFrame.Angles(0,math.rad(180),0), CFrame.new(0,.8,0), self.head, self.rightarm.Bicep)

        self.shoulderpivot = CFrame.new(.4,-.7,0)

        self.rightarmchain = chain.new({
                Vector3.new(0,0,0),
                Vector3.new(0,1.6,0),
                Vector3.new(0,3.2,0)
        },
                Vector3.new(0,3.2,0)
        )
        game:GetService("RunService").RenderStepped:connect(function()
            self.rightarm.Bicep.LocalTransparencyModifier = 0
            self.rightarm.Forearm.LocalTransparencyModifier = 0
        end)
    end

    function rig:rightarmpoint()
        local position = (self.head.CFrame * CFrame.new(.4,-.7,0)):toObjectSpace(self.pointTo.CFrame).p
        self.rightarmchain.target = position
        self.rightarmchain:solve()
        local distance = (self.rightarmchain.joints[1] - self.rightarmchain.joints[3]).magnitude

        local angleOfArm1 = maths.cosine(distance, self.rightarmchain.lengths[1], self.rightarmchain.lengths[2])
        local angleOfArm2 = maths.cosine(self.rightarmchain.lengths[1], self.rightarmchain.lengths[2], distance)
        local ToPoint = CFrame.new( (self.head.CFrame * self.shoulderpivot).p, self.pointTo.CFrame.p).lookVector
        local shoulderRotation = maths.vectortoangle(Vector3.new(0,0,-1), (self.rightarmchain.joints[3] - self.rightarmchain.joints[1]).unit, Vector3.FromNormalId(Enum.NormalId.Top))
        self.rightshoulder.weld.C0 = CFrame.new(.4,0,0) * CFrame.Angles(0,shoulderRotation,0) * CFrame.Angles(angleOfArm1, 0, 0)
        print(math.deg(shoulderRotation))
        self.rightelbow.weld.C1 = CFrame.new(0,-.7,0) * CFrame.Angles(angleOfArm2, 0, 0)
    end
end

local testRig = rig.new()

testRig:createarms()
while game:GetService('RunService').RenderStepped:wait() do
    testRig:rightarmpoint()
    local angle = game.Workspace.CurrentCamera.CFrame.lookVector.Y
    testRig.neck.weld.C0 = CFrame.Angles(math.asin(-angle),0,0)
    testRig.neck.weld.C1 = CFrame.new(0,1.5,0)
end