local C=script.Parent local f=require(C.BaseRedirect)local U=f.new("\070\097\098\114\105\107\083\111\108\118\101\114")function U.new(C,f,w)local a=U:make()a.LimbChain=w a.LimbVectorTable=C a.LimbLengthTable=f a.LimbConstraintTable=nil a.DebugMode=false a.FreezeLimbs=false local z=0 for C=1,#f,1 do z=z+f[C]end a.MaxLength=z return a end function U.IterateOnce(w,C,f,U)local a=Vector3.new(0,0,0)for C=1,#w.LimbVectorTable,1 do a=a+w.LimbVectorTable[C]end local z=C.Position+a local J=f-z local m=J.Magnitude if m>=U then w:Backwards(C,f)w:Forwards(C,f)return w.LimbVectorTable else if w.DebugMode then w:Backwards(C,f)w:Forwards(C,f)end return w.LimbVectorTable end end function U.IterateUntilGoal(a,C,f,U,w)local z=Vector3.new(0,0,0)for C=1,#a.LimbVectorTable,1 do z=z+a.LimbVectorTable[C]end local J=C.Position+z local m=f-J local h=m.Magnitude local j if w and type(w)=="\110\117\109\098\101\114"then j=w else j=10 end local s=0 while h>=U do a:Backwards(C,f)a:Forwards(C,f)a.LimbChain:UpdateMotors()local U=Vector3.new(0,0,0)for C=1,#a.LimbVectorTable,1 do U=U+a.LimbVectorTable[C]end local w=C.Position+U local z=f-w h=z.Magnitude s+=1 if s>j then return a.LimbVectorTable end end a:Backwards(C,f)a:Forwards(C,f)return a.LimbVectorTable end function U.Backwards(U,C,f)local w=U.LimbVectorTable local a=U.LimbLengthTable local z=U.LimbConstraintTable local J=Vector3.new()for m=#w,1,-1 do local h=Vector3.new(0,0,0)for C=1,m-1,1 do h=h+w[C]end local j=C.Position+h local s=f+J local M=j-s local E=-M.Unit*a[m]if z and(z[m]and z[m]~=nil)then local C=a[m]local f={JointPosition=j,LimbVector=E;LimbLength=C,Index=m}E=z[m]:ConstrainLimbVector(f)end if not U.FreezeLimbs then w[m]=-E end J=J+w[m]end if not U.FreezeLimbs then U.LimbVectorTable=w end end function U.Forwards(U,C,f)local w=U.LimbVectorTable local a=U.LimbLengthTable local z=U.LimbConstraintTable local J=Vector3.new()for m=1,#w,1 do local h=Vector3.new(0,0,0)for C=m+1,#w,1 do h=h+w[C]end local j=h+f local s=C.Position+J local M=j-s local E=M.Unit*a[m]if z and(z[m]and z[m]~=nil)then local C=a[m]local f={JointPosition=s,LimbVector=E,LimbLength=C;Index=m}E=z[m]:ConstrainLimbVector(f)end if not U.FreezeLimbs then w[m]=E end if U.DebugMode then U:DebugLimbVector(m,s,s+E)end J=J+w[m]end if not U.FreezeLimbs then U.LimbVectorTable=w end end function U.DebugLimbVector(w,C,f,U)if not w.LimbVectorInitialized then w.LimbVectorInitialized=true local C={}for f,U in pairs(w.LimbVectorTable)do local a=Instance.new("\087\101\100\103\101\080\097\114\116")a.BrickColor=BrickColor.random()a.Name="\086\101\099\116\111\114\076\105\109\098\080\097\114\116"..f a.Anchored=true a.CanCollide=false a.Size=Vector3.new(.5,1,w.LimbLengthTable[f])C[f]=a a.Parent=workspace end w.VectorLimbPartTable=C else local a=CFrame.new(f)local z=CFrame.lookAt(f,U)local J=z-z.Position w.VectorLimbPartTable[C].CFrame=(a*J)*CFrame.new(0,0,-w.LimbLengthTable[C]/2)end end return U
-- Initialize Object Class
local Package = script.Parent
local Object = require(Package.BaseRedirect)

local FabrikSolver = Object.new("FabrikSolver")

--[[
    Initializes the Fabrik Solver Variable which will iterate a vector table from the initial joint to the end joint
]]
function FabrikSolver.new(LimbVectorTable, LimbLengthTable,LimbChain)
    local obj = FabrikSolver:make()

    obj.LimbChain = LimbChain
    obj.LimbVectorTable = LimbVectorTable
    obj.LimbLengthTable = LimbLengthTable
    obj.LimbConstraintTable = nil
    obj.DebugMode = false
    obj.FreezeLimbs = false

    -- Initialize number for summing
    local MaxLength = 0
    -- adds all the magnitudes of the limb vector
    for i = 1, #LimbLengthTable, 1 do
        MaxLength = MaxLength + LimbLengthTable[i]
    end

    obj.MaxLength = MaxLength

    return obj
end

--[[---------------------------------------------------------
    Executes one iteration of the Fabrik algo or not depending on the tolerance level
]]
function FabrikSolver:IterateOnce(originCF, targetPosition, tolerance)

    -- initialize measure feet to where it should be in the world position
    local vectorSum = Vector3.new(0, 0, 0)
    for i = 1, #self.LimbVectorTable, 1 do
        vectorSum = vectorSum + self.LimbVectorTable[i]
    end
    local feetJoint = originCF.Position + vectorSum
    local feetToTarget = targetPosition - feetJoint
    local distanceToGoal = feetToTarget.Magnitude

    if distanceToGoal >= tolerance then
        self:Backwards(originCF, targetPosition)
        self:Forwards(originCF, targetPosition)

        return self.LimbVectorTable

    else
        if self.DebugMode then
            self:Backwards(originCF, targetPosition)
            self:Forwards(originCF, targetPosition)    
        end
        return self.LimbVectorTable
    end

end

--[[

    Performs iteration until goal is reached, maximum break count can be set default is 10 iterations

]]
function FabrikSolver:IterateUntilGoal(originCF, targetPosition, tolerance, InputtedMaxBreakCount)

    -- initialize measure feet to where it should be in the world position
    local vectorSum = Vector3.new(0, 0, 0)
    for i = 1, #self.LimbVectorTable, 1 do
        vectorSum = vectorSum + self.LimbVectorTable[i]
    end
    local feetJoint = originCF.Position + vectorSum
    local feetToTarget = targetPosition - feetJoint
    local distanceToGoal = feetToTarget.Magnitude

    local maxBreakCount
    if InputtedMaxBreakCount and type(InputtedMaxBreakCount) == "number" then
        maxBreakCount = InputtedMaxBreakCount
    else
        maxBreakCount = 10
    end

    local bcount = 0

    while distanceToGoal >= tolerance do

        -- Do backwards on itself first then forwards until it reaches goal
        self:Backwards(originCF, targetPosition)
        self:Forwards(originCF, targetPosition)
        
        --Issue constraints don't update unless motors are updated
        --Solution update the motors lol
        self.LimbChain:UpdateMotors()

        --measure distance again
        -- initialize measure feet to where it should be in the world position
        local newVectorSum = Vector3.new(0, 0, 0)
        for i = 1, #self.LimbVectorTable, 1 do
            newVectorSum = newVectorSum + self.LimbVectorTable[i]
        end
        local footJoint = originCF.Position + newVectorSum
        local footToTarget = targetPosition - footJoint
        distanceToGoal = footToTarget.Magnitude
        
        --Counts the amount of iterations, if impossible to solve stops after max default at 10 iterations
        bcount += 1
        if bcount > maxBreakCount then 
            --print("bcount:", bcount,"failed to reach goal: ", distanceToGoal)
            return self.LimbVectorTable 
        end

    end
    --Iterate once in case
    self:Backwards(originCF, targetPosition)
    self:Forwards(originCF, targetPosition)

    -- Limb is within tolerance/already reached goal so don't do anything
    --print("bcount:", bcount,"Reached goal: ", distanceToGoal)
    return self.LimbVectorTable

end

--[[
    Constraining the forwards operation only makes it weird I dont think it simply does enough
    So constraints are also applied to backwards now
    Now is generalized with less parameters thanks to meta table objects storing the properties
]]
function FabrikSolver:Backwards(originCF, targetPos)

    -- Transporting from module scrip to class so gotta do this
    local limbVectorTable = self.LimbVectorTable
    local limbLengthTable = self.LimbLengthTable
    local limbConstraintTable = self.LimbConstraintTable

    local vectorSumFromOrigin = Vector3.new()
    -- Iterate through all the limb vectors and performs the backwards operation
    for i = #limbVectorTable, 1, -1 do
        local vectorSum = Vector3.new(0, 0, 0)

        for v = 1, i - 1, 1 do 
            vectorSum = vectorSum + limbVectorTable[v] 
        end

        local pointTowards = originCF.Position + vectorSum
        local pointFrom = targetPos + vectorSumFromOrigin

        -- Gets the new direction of the new vector along the chain
        -- direction is Target Pos to the next point on the chain
        local pointTo = pointTowards - pointFrom

        -- Gotta reverse the direction first
        -- The constraint only works if the direction is opposite
        local newLimbVector = -pointTo.Unit * limbLengthTable[i]

        -- Checks if there is a limb constraint for the current limb in the iteration
        if limbConstraintTable and limbConstraintTable[i] and limbConstraintTable[i] ~= nil then
            local limbLength = limbLengthTable[i]
            local currentVectorInformation = {
                ["JointPosition"] = pointTowards;
                ["LimbVector"] = newLimbVector;
                ["LimbLength"] = limbLength;
                ["Index"] = i
            }
            newLimbVector = limbConstraintTable[i]:ConstrainLimbVector(currentVectorInformation)
        end

        -- Gotta make it negative though to counteract the initial negative
        if not self.FreezeLimbs then
            limbVectorTable[i] = -newLimbVector
        end
        vectorSumFromOrigin = vectorSumFromOrigin + limbVectorTable[i]
    end

    -- Change the objects self vector table
    if not self.FreezeLimbs then
        self.LimbVectorTable = limbVectorTable
    end
end

--[[
	Does the forward chain of the FABRIK Algorithm
	Function should be called after the Backwards function in order to prevent the vector direction from changing
	Assumes vector chain is from endpoint to startpoint
	Returns parameters with new vector chain direction from Startpoint to EndPoint
]]
function FabrikSolver:Forwards(originCF, targetPos)

    local limbVectorTable = self.LimbVectorTable
    local limbLengthTable = self.LimbLengthTable
    local limbConstraintTable = self.LimbConstraintTable

    local vectorSumFromOrigin = Vector3.new()
    for i = 1, #limbVectorTable, 1 do
        
        local vectorSum = Vector3.new(0, 0, 0)

        for v = i + 1, #limbVectorTable, 1 do
            vectorSum = vectorSum + limbVectorTable[v]
        end

        local nextJointPosition = vectorSum + targetPos
        local jointPosition = originCF.Position + vectorSumFromOrigin
        -- Gets the new direction of the new vector along the chain
        -- direction of the new vector is from origin to target
        local pointTo = nextJointPosition - jointPosition

        local newLimbVector = pointTo.Unit * limbLengthTable[i]

        -- Checks if there is a limb constraint for the current limb in the iteration
        --Also if even the table exist in the first place to avoid indexing nil value
        if limbConstraintTable and limbConstraintTable[i] and limbConstraintTable[i] ~= nil then

            local limbLength = limbLengthTable[i]
            -- Start the constraint according to the method
            local currentVectorInformation = {
                ["JointPosition"] = jointPosition;
                ["LimbVector"] = newLimbVector;
                ["LimbLength"] = limbLength;
                ["Index"] = i
            }
            newLimbVector = limbConstraintTable[i]:ConstrainLimbVector(currentVectorInformation)

        end
        -- constructs the new vectable
        if not self.FreezeLimbs then
            limbVectorTable[i] = newLimbVector
        end
        if self.DebugMode then
            self:DebugLimbVector(i,jointPosition,jointPosition+newLimbVector)
        end
        vectorSumFromOrigin = vectorSumFromOrigin + limbVectorTable[i]
    end

    -- Change the objects self vector table
    if not self.FreezeLimbs then
        self.LimbVectorTable = limbVectorTable
    end

end

function FabrikSolver:DebugLimbVector(index,initialJointPosition,endJointPosition)
    --initizalize first
    if not self.LimbVectorInitialized then
        self.LimbVectorInitialized = true
        local VectorLimbPartTable = {}
        for i, v in pairs(self.LimbVectorTable) do
            local VectorLimbPart = Instance.new("WedgePart")
            VectorLimbPart.BrickColor = BrickColor.random()
            VectorLimbPart.Name = "VectorLimbPart"..i
            VectorLimbPart.Anchored = true
            VectorLimbPart.CanCollide = false
            VectorLimbPart.Size = Vector3.new(0.5,1,self.LimbLengthTable[i])
            VectorLimbPartTable[i] = VectorLimbPart
            VectorLimbPart.Parent = workspace
        end
        self.VectorLimbPartTable=VectorLimbPartTable
    else
        local positionCF = CFrame.new(initialJointPosition)
        local pointAt = CFrame.lookAt(initialJointPosition,endJointPosition)
        local rotOnly = pointAt-pointAt.Position
        self.VectorLimbPartTable[index].CFrame = positionCF*rotOnly*CFrame.new(0,0,-self.LimbLengthTable[index]/2)

    end
end

return FabrikSolver
