local Matrix = {} -- the matrix class
local Vector = {} -- the vector class

-- Vector prototype
Vector.__index = Vector

--- Create a new 1xn Vector object.
--- @param vector table<number> The 1xn vector data (row-major)
--- @return Vector A 1xn vector
function Vector:new(vec)
    local A = type(vec) == "table"
    local n = #vec
    A = A and n > 0
    if A == true then 
        for c = 1, n do 
            A = A and type(vec[c]) == "number"
        end
    end
    assert(A, "A Vector object must be a 1xn table of numbers, where n>0.")
    return setmetatable(vec, self)
end

--- Pretty print for Vector object
--- @return string
function Vector:__tostring()
    return ("Vector{%.12f" .. string.rep(",%.12f", #self - 1) .. "}"):format(table.unpack(self))
end

--- Deep copy of Vector object
--- @return Vector A deep copy of self
function Vector:deep_copy()
    local n = #self
    local copy = {}
    for i = 1, n do
        copy[i] = self[i]
    end
    return Vector:new(copy)
end

--- Convert Vector to table
--- @return table<number> A table representation of self
function Vector:to_table()
    local n = #self
    local t = {}
    for i = 1, n do
        t[i] = self[i]
    end
    return t
end

--- Homogeneous addition of Vector objects
--- @param other Vector A vector of same size
--- @return Vector The sum of self and other
function Vector:hadd(other)
    assert(getmetatable(other) == Vector, "You tried adding a vector to a non-vector (homogeneous).")
    local lo = #other
    local ls = #self
    assert(lo == ls, "You tried adding vectors of different sizes (homogeneous).")
    assert(lo > 1, "Homogeneous addition only works on size two or more.")
    local V = {}
    for i = 1, lo - 1 do 
        V[i] = self[i] + other[i]
    end
    V[lo] = self[lo]
    return Vector:new(V)
end

--- Addition of Vector objects
--- @param other Vector A vector of same size
--- @return Vector The sum of self and other
function Vector:add(other)
    assert(getmetatable(other) == Vector, "You tried adding a vector to a non-vector.")
    local lo = #other
    local ls = #self
    assert(lo == ls, "You tried adding vectors of different sizes.")
    assert(lo > 1, "Addition only works on size two or more.")
    local V = {}
    for i = 1, lo do 
        V[i] = self[i] + other[i]
    end
    return Vector:new(V)
end

--- Homogeneous subtraction of Vector objects
--- @param other Vector A vector of same size
--- @return Vector The difference of self and other
function Vector:hsub(other)
    assert(getmetatable(other) == Vector, "You tried subtracting a vector from a non-vector.")
    local lo = #other
    local ls = #self
    assert(lo == ls, "You tried subtracting vectors of different sizes (homogeneous).")
    assert(lo > 1, "Homogeneous subtraction only works on size two or more.")
    local V = {}
    for i = 1, lo - 1 do 
        V[i] = self[i] - other[i]
    end
    V[lo] = self[lo]
    return Vector:new(V)
end

--- Subtraction of Vector objects
--- @param other Vector A vector of same size
--- @return Vector The difference of self and other
function Vector:sub(other)
    assert(getmetatable(other) == Vector, "You tried subtracting a vector from a non-vector.")
    local lo = #other
    local ls = #self
    assert(lo == ls, "You tried subtracting vectors of different sizes.")
    assert(lo > 1, "Subtraction only works on size two or more.")
    local V = {}
    for i = 1, lo do 
        V[i] = self[i] - other[i]
    end
    return Vector:new(V)
end

--- Homogeneous scaling of Vector objects
--- @param scalar number The scalar
--- @return Vector The scale of self by num
function Vector:hscale(scalar)
    assert(type(scalar) == "number", "You tried scaling a vector by a non-number.")
    local ls = #self
    assert(ls > 1, "Homogeneous scaling only works on size two or more.")
    local V = {}
    for i = 1, ls - 1 do 
        V[i] = self[i] * scalar
    end
    V[ls] = self[ls]
    return Vector:new(V)
end

--- Scaling of Vector objects
--- @param scalar number The scalar
--- @return Vector The scale of self by num
function Vector:scale(scalar)
    assert(type(scalar) == "number", "You tried scaling a vector by a non-number.")
    local ls = #self
    assert(ls > 1, "Scaling only works on size two or more.")
    local V = {}
    for i = 1, ls do 
        V[i] = self[i] * scalar
    end
    return Vector:new(V)
end

--- Homogeneous inner product of Vector objects
--- @param other Vector The RHS
--- @return number The homogeneous inner product of self with other
function Vector:hinner(other)
    assert(getmetatable(other) == Vector, "You tried taking the inner product of a vector with a non-vector (homogeneous).")
    local ls = #self
    local lo = #other 
    assert(ls == lo, "You tried taking the inner product of different sized vectors (homogeneous).")
    assert(ls > 1, "Homogeneous inner product only works on size two or more.")
    local result = 0
    for i = 1, ls - 1 do
        result = result + self[i] * other[i]
    end
    return result
end

--- Homogeneous hypercross product of Vector objects
--- @param ... Vector Additional vectors
--- @return Vector The homogeneous hypercross product of self with the additional vectors
function Vector:hhypercross(...)
    local args = { ... }
    local n = #self - 1
    assert(n > 1, "Homogeneous hypercross requires dimension >= 2.")
    assert(#args == n - 2,
        "Expected " .. (n - 2) .. " additional vectors.")

    for _, v in ipairs(args) do
        assert(getmetatable(v) == Vector, "All arguments must be Vector objects.")
        assert(#v == #self, "All vectors must have the same size.")
    end
    local w = self[n + 1]
    local M = {}
    for i = 1, n do
        local e = {}
        for j = 1, n do
            e[j] = (i == j) and 1 or 0
        end
        e[n + 1] = 0

        M[i] = {}
        M[i][1] = Vector:new(e)
        M[i][2] = self[i]

        for j = 1, n - 2 do
            M[i][j + 2] = args[j][i]
        end
    end
    local result = Matrix:new(M):det("column", 1)
    result[n + 1] = w
    return Vector:new(result)
end

--- Homogeneous normalization of Vector objects
--- @return Vector The homogeneous normalization of self
function Vector:hnormalize()
    local ls = #self
    assert(ls > 1, "Homogeneous normalization only works on size two or more.")
    local len = 0
    for i = 1, ls - 1 do 
        len = len + self[i] * self[i]
    end
    len = math.sqrt(len)
    local V = {}
    for i = 1, ls - 1 do 
        V[i] = self[i] / len
    end
    V[ls] = self[ls]
    return Vector:new(V)
end

--- Homogeneous distance between Vector objects
--- @param other Vector A vector of same size
--- @return number The homogeneous distance between self and other
function Vector:hdistance(other)
    assert(getmetatable(other) == Vector, "You tried taking the distance between a vector and a non-vector (homogeneous).")
    local ls = #self
    local lo = #other 
    assert(ls == lo, "You tried taking the distance between different sized vectors (homogeneous).")
    assert(ls > 1, "Homogeneous distance only works on size two or more.")
    local result = 0
    for i = 1, ls - 1 do
        result = result + (self[i] - other[i])^2
    end
    return math.sqrt(result)
end

--- Homogeneous projection of Vector objects
--- @param other Vector A vector of same size
--- @return Vector The homogeneous projection of self onto other
function Vector:hproject_onto(other)
    assert(getmetatable(other) == Vector, "You tried projecting a vector onto a non-vector (homogeneous).")
    local ls = #self
    local lo = #other 
    assert(ls == lo, "You tried projecting between different sized vectors (homogeneous).")
    assert(ls > 1, "Homogeneous vector projection only works on size two or more.")
    local denom = other:hinner(other)
    local scalar = self:hinner(other) / denom
    return other:hscale(scalar)
end

--- Homogeneous orthogonal projection of Vector objects onto a plane
--- @param plane_basis Matrix A 3xN matrix representing the plane basis in homogeneous coordinates
--- @return Vector The homogeneous orthogonal projection of self onto the plane
function Vector:horthogonal_projection_onto_plane(plane_basis)
    assert(getmetatable(plane_basis) == Matrix, "You tried projecting onto a non-matrix (homogeneous).")
    local TO = Vector:new(plane_basis[1])
    local TU = Vector:new(plane_basis[2])
    local TV = Vector:new(plane_basis[3])
    local TW = TU:hhypercross(TV)
    local TOS = self:hsub(TO)
    if TW:hinner(TOS) < 0 then 
        TW = TW:hscale(-1)
    end
    local proj = TOS:hproject_onto(TW)
    return self:hsub(proj)
end

--- Homogeneous norm of Vector objects
--- @return number The homogeneous norm of self
function Vector:hnorm()
    assert(#self > 1, "Homogeneous norm only works on size two or more.")
    local sum = 0
    for i = 1, #self - 1 do 
        sum = sum + self[i] * self[i]
    end
    return math.sqrt(sum)
end

--- Homogeneous reciprocation of Vector objects
--- @return Vector The homogeneous reciprocation of self
function Vector:reciprocate_by_homogenous()
    assert(#self > 1, "Reciprocating by homogeneous only works on size two or more.")
    local w = self[#self]
    local V = {}
    for i = 1, #self - 1 do 
        V[i] = self[i] / (w * w)
    end
    V[#self] = 1
    return Vector:new(V)
end

--- Find an arbitrary vector orthogonal to self
--- @return Vector An arbitrary vector orthogonal to self
function Vector:orthogonal_vector()
    local v 
    if (
        math.abs(self[1]) > 1e-3
        and math.abs(self[2]) > 1e-3
        and math.abs(self[3]) > 1e-3
    ) then
        v = self:hhypercross(Vector:new{0,1,0,1})
    else
        v = self:hhypercross(Vector:new{1,0,0,1})
    end
    return v
end

--- Multiply Vector by Matrix
--- @param matrix Matrix The matrix
--- @return Vector The product of self and matrix
function Vector:multiply(matrix)
    assert(getmetatable(matrix) == Matrix, "You tried multiplying a vector by a non-matrix.")
    local M = Matrix:new{{table.unpack(self)}}:multiply(matrix)
    return Vector:new(M[1])
end













































--- Check if two homogeneous vectors intersect (are the same point)
--- @param other Vector A vector of same size 
--- @return boolean True if they intersect, false otherwise
function Vector:hpoint_point_intersecting(other)
    return self:hdistance(other) < 1e-12
end


--- Check if a homogeneous point is in the triangular prism defined by triangle T
--- @param T Matrix A 3xN matrix representing the triangle vertices in homogeneous coordinates
--- @return boolean True if the point is in the triangular prism, false otherwise
function Vector:hpoint_in_triangular_prism(T)
    local A = Vector:new(T[1])
    local B = Vector:new(T[2])
    local C = Vector:new(T[3])

    local v0 = C:hsub(A)
    local v1 = B:hsub(A)
    local v2 = self:hsub(A)

    local d00 = v0:hinner(v0)
    local d01 = v0:hinner(v1)
    local d11 = v1:hinner(v1)
    local d20 = v2:hinner(v0)
    local d21 = v2:hinner(v1)
    local denom = d00 * d11 - d01 * d01
    local v = (d11 * d20 - d01 * d21) / denom
    local w = (d00 * d21 - d01 * d20) / denom
    local u = 1 - v - w
    local eps = 1e-12
    return u >= -3*eps and v >= -3*eps and w >= -3*eps
end

--- Check if a homogeneous point intersects a homogeneous triangle
--- @param T Matrix A 3xN matrix representing the triangle vertices in homogeneous coordinates
--- @return boolean True if they intersect, false otherwise
function Vector:hpoint_triangle_intersecting(T)
    local Tbasis = T:deep_copy():hto_basis()
    local S = self:horthogonal_projection_onto_plane(Tbasis)
    -- if the point is inside the triangular prism and intersects its 
    -- orthogonal projection onto the triangle's plane, then it 
    -- intersects the triangle
    return S:hpoint_in_triangular_prism(T) and self:hpoint_point_intersecting(S)
end


--- Check occlusion sort of a homogeneous point with a homogeneous point
--- @param P Vector A vector of same size
--- @return boolean|nil True if self is in front of P, false if behind, nil if no occlusion
function Vector:hpoint_point_occlusion_sort(P)
    if not self:hpoint_point_intersecting(P) then
        if Vector:new{self[1], self[2], 0, 1}:hpoint_point_intersecting(Vector:new{P[1], P[2], 0, 1}) then
            return self[3] < P[3]
        end
    end
    return nil 
end

--- Check collinearity of two homogeneous vectors
--- @param other Vector A vector of same size
--- @return boolean True if they are collinear, false otherwise
function Vector:hcollinear(other)
    assert(getmetatable(other) == Vector, "You tried checking collinearity of a vector with a non-vector (homogeneous).")
    local ls = #self
    local lo = #other 
    assert(ls == lo, "You tried checking collinearity of different sized vectors (homogeneous).")
    assert(ls > 1, "Homogeneous vector collinearity only works on size two or more.")
    local cross = self:hhypercross(other)
    return cross:hnorm() < 1e-12
end

--- Check opposite direction of two homogeneous vectors
--- @param other Vector A vector of same size
--- @return boolean True if they are oppositely directed, false otherwise
function Vector:hoppositely_directed(other)
    assert(self:hcollinear(other), "You tried checking opposite direction of non-collinear vectors (homogeneous).")
    local ls = #self
    assert(ls > 1, "Homogeneous vector opposite direction only works on size two or more.")
    local dot = self:hinner(other)
    return dot < 0
end


--- Check occlusion sort of a homogeneous point with a homogeneous line segment
--- @param L Matrix A matrix of two vectors
--- @return boolean|nil True if self is in front of L, false if behind, nil if no occlusion
function Vector:hpoint_line_segment_occlusion_sort(L)
    local line = L:deep_copy() -- the line segment
    local LA = line:hto_basis() -- the basis of the line through the segment
    local LO = Vector:new(LA[1]) -- the line's origin
    local OP = self:hsub(LO) -- the vector from the line's origin to our point
    local LU = Vector:new(LA[2]) -- the line's direction vector
    local proj = OP:hproject_onto(LU) -- the projection of our vector onto the line's basis
    local true_proj = LO:hadd(proj) -- the sum of the origin and the vector, producing the true point on the line
    local F = Vector:new{self[1], self[2], 0, 1} -- projection of self onto xy
    local G = Vector:new{true_proj[1], true_proj[2], 0, 1} -- projection of the point on the line onto xy
    if F:hpoint_point_intersecting(G) then -- their vertical projections must overlap
        local temp = 1 -- the preserved direction
        if proj:hoppositely_directed(LU) then
            temp = -1 
        end
        local test = temp * proj:hnorm() / LU:hnorm() -- the coordinate of our point on the line segment
        local eps = 1e-12
        if (eps < test and test < 1 - eps) then -- the point must be on the line segment
            return self:hpoint_point_occlusion_sort(true_proj)
        end
    else
        return nil
    end
end

--- Check occlusion sort of a homogeneous point with a homogeneous triangle
--- @param T Matrix A 3xN matrix representing the triangle vertices in homogeneous coordinates
--- @return boolean|nil True if self is in front of T, false if behind, nil if no occlusion
function Vector:hpoint_triangle_occlusion_sort(T)
    local P1 = Vector:new{self[1], self[2], 0, 1}
    local T1 = Vector:new{T[1][1], T[1][2], 0, 1}
    local T2 = Vector:new{T[2][1], T[2][2], 0, 1}
    local T3 = Vector:new{T[3][1], T[3][2], 0, 1}
    local TP = Matrix:new{
        {T1[1], T1[2], 0, 1},
        {T2[1], T2[2], 0, 1},
        {T3[1], T3[2], 0, 1}
    }
    local Ta = Vector:new(T[1])
    local Tb = Vector:new(T[2])
    local Tc = Vector:new(T[3])

    if self:hpoint_point_intersecting(Ta) then return nil end
    if self:hpoint_point_intersecting(Tb) then return nil end
    if self:hpoint_point_intersecting(Tc) then return nil end
    local eps = 1e-12
    if P1:hpoint_in_triangular_prism(TP) then
        local vu = Tb:hsub(Ta)
        local vv = Tc:hsub(Ta)
        local sol = Matrix:new{
            {vu[1], vu[2]},
            {vv[1], vv[2]},
            {P1:hsub(Ta)[1], P1:hsub(Ta)[2]}
        }:column_reduction()
        local t, s
        if sol then 
            t, s = sol[1], sol[2]
        else return nil end
        if (
            -eps < t and t < 1 + eps
            and -eps < s and s < 1 + eps
        ) then 
            local a = Ta:hadd(vu:hscale(t)):hadd(vv:hscale(s))
            return self:hpoint_point_occlusion_sort(a)
        else
            return nil
        end
    end
    return nil
end













































-- Matrix prototype
Matrix.__index = Matrix

--- Create a new Matrix object.
--- @param matrix table<number|Vector> The matrix data (row-major)
--- @return Matrix
function Matrix:new(matrix)
    assert(type(matrix) == "table", "A Matrix object must be a table of rows.")
    local l = #matrix 
    -- assert(l > 1, "A Matrix must have at least two rows.")
    local p = #matrix[1]
    -- assert(p > 1, "A Matrix must have at least two columns")
    for i = 1, l do
        assert(type(matrix[i]) == "table", "A Matrix row must be a table.")
        local m = #matrix[i]
        assert(m == p, "All Matrix rows must have the same number of columns.")
        for j = 1, m do 
            local g = getmetatable(matrix[i][j]) == Vector
            assert(
                (
                    type(matrix[i][j]) == "number"
                    or g
                ), "A Matrix can only contain numbers or Vectors"
            )
            if g and i == 1 then
                for k = 2, l do 
                    assert(#matrix[k][j] == #matrix[1][j], "All Vectors in a Matrix column must have the same size.")
                end
            end
        end
    end
    return setmetatable(matrix, self)
end


--- Print a Matrix object
--- @return string
function Matrix:__tostring()
    local s = "Matrix{\n"
    local l = #self
    for i = 1, l do 
        s = s .. "\t{"
        local m = #self[i]
        for j = 1, m do
            if type(self[i][j]) == "number" then
                s = s .. string.format("%.12f", self[i][j])
            else
                s = s .. tostring(self[i][j])
            end
            if j < m then s = s .. "," end
        end
        s = s .. "}"
        if i < l then s = s .. ",\n" end
    end
    s = s .. "\n}"
    return s
end

--- swap two columns of the Matrix
--- @param c1 number The first column
--- @param c2 number The second column
function Matrix:swap_columns(c1, c2)
    local l = #self
    for i = 1, l do 
        local t = self[i][c1]
        self[i][c1] = self[i][c2]
        self[i][c2] = t
    end
end

--- add a scalar multiple of one column to another column
--- @param src_col number The source column
--- @param scalar number The scalar multiple
--- @param dest_col number The destination column
function Matrix:add_scalar_multiple_of_column(src_col, scalar, dest_col)
    local l = #self
    for i = 1, l do 
        self[i][dest_col] = self[i][dest_col] + scalar * self[i][src_col]
    end
end

--- scale a column by a scalar
--- @param col number The column
--- @param scalar number The scalar
function Matrix:scale_column(col, scalar)
    local l = #self
    for i = 1, l do 
        self[i][col] = self[i][col] * scalar
    end
end

--- Deep copy of Matrix object
--- @return Matrix A deep copy of self
function Matrix:deep_copy()
    local numrows = #self
    local numcols = #self[1]
    local deep_copy = {}
    for i = 1, numrows do
        deep_copy[i] = {}
        for j = 1, numcols do
            if getmetatable(self[i][j]) == Vector then
                deep_copy[i][j] = Vector:new{table.unpack(self[i][j])}
            else
                deep_copy[i][j] = self[i][j]
            end
        end
    end
    return Matrix:new(deep_copy)
end

function Matrix:to_table()
    local numrows = #self
    local numcols = #self[1]
    local t = {}
    for i = 1, numrows do
        t[i] = {}
        for j = 1, numcols do
            if getmetatable(self[i][j]) == Vector then
                t[i][j] = self[i][j]:to_table()
            else
                t[i][j] = self[i][j]
            end
        end
    end
    return t
end


--- Columnn reduction of the Matrix to RREF form
--- @return Vector The solution vector (last column of RREF)
function Matrix:column_reduction()
    local A = self:deep_copy()
    local numrows = #A
    local numcols = #A[1]
    local eps = 1e-12

    local pivot_row = 1

    for col = 1, numcols do
        if pivot_row > numrows then
            break
        end

        -- Find a pivot in this column at or below pivot_row
        local pivot_row_found = nil
        for r = pivot_row, numrows do
            if math.abs(A[r][col]) > eps then
                pivot_row_found = r
                break
            end
        end

        if not pivot_row_found then
            -- No pivot in this column
            goto continue
        end

        -- Move pivot column into position pivot_row
        if col ~= pivot_row then
            A:swap_columns(col, pivot_row)
            col = pivot_row
        end

        -- Normalize pivot column
        local pivot_val = A[pivot_row][col]
        A:scale_column(col, 1 / pivot_val)

        -- Eliminate pivot_row entry from all other columns
        for j = 1, numcols do
            if j ~= col then
                local factor = -A[pivot_row][j]
                if math.abs(factor) > eps then
                    A:add_scalar_multiple_of_column(col, factor, j)
                end
            end
        end

        pivot_row = pivot_row + 1

        ::continue::
    end
    local ans = Vector:new(A[#A])
    for _, val in ipairs(ans) do
        if val ~= val then
            return nil
        end
    end
    return ans
end

--- Transpose of Matrix object
--- @return Matrix The transposed matrix
function Matrix:transpose()
    local numrows = #self
    local numcols = #self[1]
    local transposed = {}
    for j = 1, numcols do
        transposed[j] = {}
        for i = 1, numrows do 
            transposed[j][i] = self[i][j]
        end
    end
    return Matrix:new(transposed)
end



--- Inverse of the Matrix (row-vector convention, column GJ)
--- @return Matrix|nil The inverse matrix, or nil if not invertible
function Matrix:inverse()
    local n = #self
    assert(n == #self[1], "Matrix must be square.")

    local eps = 1e-12

    -- Working copy of A
    local A = Matrix:new(self:to_table())

    -- Accumulator for LEFT inverse: manually build identity
    local L_table = {}
    for i = 1, n do
        L_table[i] = {}
        for j = 1, n do
            L_table[i][j] = (i == j) and 1 or 0
        end
    end
    local L = Matrix:new(L_table)

    for pivot = 1, n do
        -- Find pivot column (allowing row scan)
        local pivot_col = nil
        local pivot_row = nil

        for c = pivot, n do
            for r = pivot, n do
                if math.abs(A[r][c]) > eps then
                    pivot_col = c
                    pivot_row = r
                    break
                end
            end
            if pivot_col then break end
        end

        if not pivot_col then
            return nil
        end

        -- Bring pivot column into place
        if pivot_col ~= pivot then
            A:swap_columns(pivot_col, pivot)
            L:swap_columns(pivot_col, pivot)
        end

        -- Bring pivot row into place
        if pivot_row ~= pivot then
            A:swap_rows(pivot_row, pivot)
            L:swap_rows(pivot_row, pivot)
        end

        -- Normalize pivot column
        local p = A[pivot][pivot]
        A:scale_column(pivot, 1 / p)
        L:scale_column(pivot, 1 / p)

        -- Eliminate pivot row from other columns
        for c = 1, n do
            if c ~= pivot then
                local k = -A[pivot][c]
                if math.abs(k) > eps then
                    A:add_scalar_multiple_of_column(pivot, k, c)
                    L:add_scalar_multiple_of_column(pivot, k, c)
                end
            end
        end
    end

    return L
end




--- Convert to a basis from a simplex
--- @return Matrix The basis matrix
function Matrix:hto_basis()
    local numrows = #self
    local numcols = #self[1]
    local deep_copy = self:deep_copy()
    for i = 1, numrows do
        if i ~= 1 then  
            for j = 1, numcols - 1 do
                deep_copy[i][j] = deep_copy[i][j] - deep_copy[1][j]
            end
        end
    end
    return deep_copy
end

--- Convert to a simplex from a basis
--- @return Matrix The simplex matrix
function Matrix:hto_simplex()
    local numrows = #self
    local numcols = #self[1]
    local deep_copy = self:deep_copy()
    for i = 1, numrows do
        if i ~= 1 then  
            for j = 1, numcols - 1 do
                deep_copy[i][j] = deep_copy[i][j] + deep_copy[1][j]
            end
        end
    end
    return deep_copy
end

--- Compute the minor of the matrix by removing row ii and column jj
--- @param ii number The row to remove
--- @param jj number The column to remove
--- @return number|Vector The minor determinant
function Matrix:minor(ii, jj)
    local numrows = #self 
    local numcols = #self[1]
    assert(numrows == numcols, "You tried to take a minor of a non-square matrix.")
    assert(numrows > 1, "You can't take a minor of a matrix which is smaller than 2x2.")
    assert(ii <= numrows and jj <= numcols, "Index out of bounds for minor.")
    local M = {}
    for i = 1, numrows do
        if i ~= ii then table.insert(M, {}) end
        local pos = i 
        if pos > ii then pos = pos - 1 end
        if i ~= ii then M[pos] = {} 
            for j = 1, numcols do 
                if j ~= jj then table.insert(M[pos], self[i][j]) end
            end 
        end
    end
    return Matrix:new(M):det("row", 1)
end
        
--- Compute the determinant of the matrix by cofactor expansion along a row or column
--- @param switch string The row/column switch
--- @param pos number The position of the row or column
--- @return number|Vector The determinant
function Matrix:det(switch, pos)
    local numrows = #self 
    local numcols = #self[1]
    assert(numrows == numcols, "You tried to take the determinant of a non-square matrix.")
    assert(switch == "row" or switch == "column", "You didn't specify \"row\" or \"column\" for the cofactor expansion of a determinant.")
    assert(pos <= numrows, "Cofactor (row or colum) position out of bounds.")
    if numrows == 2 then 
        local A = getmetatable(self[1][1])
        local B = getmetatable(self[2][1])
        if A == Vector and B == A then 
            return A:scale(self[2][2]):add(B:scale(self[1][2]))
        end
        A = getmetatable(self[1][2])
        B = getmetatable(self[2][2])
        if A == Vector and B == A then 
            return A:scale(self[2][1]):add(B:scale(self[1][1]))
        end
        return self[1][1] * self[2][2] - self[1][2] * self[2][1]
    end
    if switch == "row" then 
        local i = pos
        local det = 0
        for j = 1, numcols do
            local sign = (-1)^(i + j)
            local minor = self:minor(i, j)
            local s = self[i][j]
            if getmetatable(s) == Vector then 
                error("Cofactor expansion along a column of vectors works, but not along a row.")
            end
            det = det + s * minor * sign
        end
        return det
    elseif switch == "column" then 
        local j = pos 
        local det 
        if getmetatable(self[1][j]) == Vector then 
            local t = {}
            for i = 1, numcols + 1 do t[i] = 0 end
            det = Vector:new(t)
        else
            det = 0
        end
        for i = 1, numrows do 
            local sign = (-1)^(i + j)
            local minor = self:minor(i, j)
            local s = self[i][j]
            if getmetatable(s) == Vector then 
                det = det:add(s:scale(minor * sign))
            else 
                det = det + s * minor * sign
            end
        end
        return det
    end 
end

--- Homogeneous reciprocation of Matrix objects
--- @return Matrix The homogeneous reciprocation of self
function Matrix:reciprocate_by_homogenous()
    local result = {}
    for i = 1, #self do
        result[i] = {}
        local w = self[i][#self[i]]
        for j = 1, #self[i] - 1 do 
            result[i][j] = self[i][j] / (w * w)
        end
        result[i][#self[i]] = 1
    end
    return Matrix:new(result)
end


--- Multiply two Matrix objects
--- @param other Matrix|Vector The RHS
--- @return Matrix|Vector The product of self and other
function Matrix:multiply(other)
    local A = self:deep_copy()
    local flag
    if getmetatable(other) == Vector then 
        other = Matrix:new{other:to_table()}
        flag = true
    end
    local Arows = #A
    local Acols = #A[1]
    local Brows = #other
    local Bcols = #other[1]
    assert(Acols == Brows, "You tried multiplying matrices with incompatible sizes.")
    local product = {}
    for row = 1, Arows do
        product[row] = {}
        for col = 1, Bcols do 
            product[row][col] = 0
            for k = 1, Acols do 
                local a = A[row][k]
                local b = other[k][col]
                product[row][col] = product[row][col] + a * b
            end
        end
    end
    if #product == 4 and #product[1] == 4 then
        return Matrix:new(product)
    elseif flag then 
        return Vector:new(product[1])
    else 
        return Matrix:new(product):reciprocate_by_homogenous()
    end
end


--- Get 3D bounding box of Matrix points
--- @return table A table with 'min' and 'max' Vector objects representing the bounding
function Matrix:get_bbox3()
    local min_x = math.huge
    local min_y = math.huge
    local min_z = math.huge
    local max_x = -math.huge
    local max_y = -math.huge
    local max_z = -math.huge
    for i, row in ipairs(self) do
        local v = Vector:new(row)
        if v[1] < min_x then min_x = v[1] end
        if v[2] < min_y then min_y = v[2] end
        if v[3] < min_z then min_z = v[3] end
        if v[1] > max_x then max_x = v[1] end
        if v[2] > max_y then max_y = v[2] end
        if v[3] > max_z then max_z = v[3] end
    end
    return {
        min = Vector:new{min_x, min_y, min_z, 1},
        max = Vector:new{max_x, max_y, max_z, 1}
    }
end

--- Get 2D bounding box of Matrix points
--- @return table A table with 'min' and 'max' Vector objects representing the bounding
function Matrix:get_bbox2()
    local min_x = math.huge
    local min_y = math.huge
    local max_x = -math.huge
    local max_y = -math.huge
    for i, row in ipairs(self) do
        local v = Vector:new(row)
        if v[1] < min_x then min_x = v[1] end
        if v[2] < min_y then min_y = v[2] end
        if v[1] > max_x then max_x = v[1] end
        if v[2] > max_y then max_y = v[2] end
    end
    return {
        min = Vector:new{min_x, min_y, 0, 1},
        max = Vector:new{max_x, max_y, 0, 1}
    }
end

--- Check if two 3D bounding boxes overlap
--- @param other Matrix The other Matrix
--- @return boolean True if they overlap, false otherwise
function Matrix:bboxes_overlap3(other)
    local a_bbox = self:get_bbox3()
    local b_bbox = other:get_bbox3()
    return not (
        a_bbox.max[1] < b_bbox.min[1]
        or a_bbox.min[1] > b_bbox.max[1]
        or a_bbox.max[2] < b_bbox.min[2]
        or a_bbox.min[2] > b_bbox.max[2]
        or a_bbox.max[3] < b_bbox.min[3]
        or a_bbox.min[3] > b_bbox.max[3]
    )
end

--- Check if two 2D bounding boxes overlap
--- @param other Matrix The other Matrix
--- @return boolean True if they overlap, false otherwise
function Matrix:bboxes_overlap2(other)
    local a_bbox = self:get_bbox2()
    local b_bbox = other:get_bbox2()
    return not (
        a_bbox.max[1] < b_bbox.min[1]
        or a_bbox.min[1] > b_bbox.max[1]
        or a_bbox.max[2] < b_bbox.min[2]
        or a_bbox.min[2] > b_bbox.max[2]
    )
end


--- Compute the homogeneous centroid of the Matrix points
--- @return Vector The homogeneous centroid
function Matrix:hcentroid()
    local ns = #self
    local dim = #self[1]
    local centroid = Vector:new(self[1])
    for i = 2, ns do
        local v = Vector:new(self[i])
        centroid = centroid:hadd(v)
    end
    centroid = centroid:hscale(1 / ns)
    return centroid
end











--- Sort points in Matrix by angle around their homogeneous centroid
--- @return Matrix The sorted points
function Matrix:hcentroid_sort()
    local num = #self
    local centroid = self:hcentroid()
    local I = self:deep_copy()
    local sum = Vector:new{0,0,0,1}
    for i = 1, num do 
        sum = sum:add(Vector:new(I[i]))
    end
    centroid = sum:scale(1/num)
    local P = Vector:new(I[1])
    local u = Vector:new(I[2]):hsub(P):hnormalize()
    local v = Vector:new(I[3]):hsub(P)
    local normal = u:hhypercross(v):hnormalize()
    v = normal:hhypercross(u):hnormalize()
    local angles = {}
    for i, p in ipairs(I) do 
        local rel = Vector:new(p):hsub(centroid)
        local x = rel:hinner(u)
        local y = rel:hinner(v)
        local angle = math.atan2(y, x)
        angles[i] = {angle = angle, index = i}
    end
    table.sort(angles, function(a, b) return a.angle < b.angle end)
    local sorted = {}
    for i, a in ipairs(angles) do 
        sorted[i] = I[a.index]
    end
    return sorted
end






--- Homogeneous occlusion sorting between simplices
--- @param L Matrix A 2xN matrix representing the line segment endpoints in homogeneous coordinates
--- @return boolean|nil True if self is in front of L, false if L is
function Matrix:hline_segment_line_segment_occlusion_sort(L)
    -- projection of self onto xy
    local L1A = Vector:new{self[1][1], self[1][2], 0, 1}
    local L1B = Vector:new{self[2][1], self[2][2], 0, 1}
    -- projection of L onto xy
    local L2A = Vector:new{L[1][1], L[1][2], 0, 1}
    local L2B = Vector:new{L[2][1], L[2][2], 0, 1}
    -- direction vectors in xy
    local L1_dir = L1B:hsub(L1A)
    local L2_dir = L2B:hsub(L2A)

    -- real self line segment in 3D
    local RL1A = Vector:new(self[1])
    local RL1B = Vector:new(self[2])
    
    -- real L line segment in 3D
    local RL2A = Vector:new(L[1])
    local RL2B = Vector:new(L[2])

    -- direction vectors in 3D
    local RL1_dir = RL1B:hsub(RL1A)
    local RL2_dir = RL2B:hsub(RL2A)

    local eps = 1e-12
    local lll = not L1_dir:hcollinear(L2_dir)
    if lll then -- if they are not collinear
        -- then we can try to find an intersection point in xy
        local sol = Matrix:new{
            {L1_dir[1], L1_dir[2]},
            {-L2_dir[1], -L2_dir[2]},
            {L2A[1] - L1A[1], L2A[2] - L1A[2]}
        }:column_reduction()
        local t, s
        if sol ~= nil then 
            t, s = sol[1], sol[2]
        else return nil end
        if (
            -eps < t and t < 1 + eps
            and -eps < s and s < 1 + eps
        ) then 
            local RL1I = RL1A:hadd((RL1B:hsub(RL1A)):hscale(t))
            local RL2I = RL2A:hadd((RL2B:hsub(RL2A)):hscale(s))
            return RL1I:hpoint_point_occlusion_sort(RL2I)
        end
    else
        local M1 = RL1A:hpoint_line_segment_occlusion_sort(L)
        if M1 ~= nil then return M1 end
        local M2 = RL1B:hpoint_line_segment_occlusion_sort(L)
        if M2 ~= nil then return M2 end
        local M3 = RL2A:hpoint_line_segment_occlusion_sort(self)
        if M3 ~= nil then return not M3 end
        local M4 = RL2B:hpoint_line_segment_occlusion_sort(self)
        if M4 ~= nil then return not M4 end
        return nil
    end
    return nil
end

--- Homogeneous occlusion sorting between a line segment and a triangle
--- @param T Matrix A 3xN matrix representing the triangle vertices in homogeneous coordinates
--- @return boolean|nil True if self is in front of T, false if T is
function Matrix:hline_segment_triangle_occlusion_sort(T)
    local points1 = {
        Vector:new(self[1]),
        Vector:new(self[2])
    }
    for _, p1 in ipairs(points1) do 
        local A = p1:hpoint_triangle_occlusion_sort(T)
        if A ~= nil then return A end
    end
    local edges2 = {
        Matrix:new{T[1], T[2]},
        Matrix:new{T[2], T[3]},
        Matrix:new{T[3], T[1]}
    }
    for _, e2 in ipairs(edges2) do 
        local A = self:hline_segment_line_segment_occlusion_sort(e2)
        if A ~= nil then return A end
    end
    return nil
end

--- Homogeneous occlusion sorting between two triangles
--- @param T Matrix A 3xN matrix representing the triangle vertices in homogeneous coordinates
--- @return boolean|nil True if self is in front of T, false if T is
function Matrix:htriangle_triangle_occlusion_sort(T)
    local edges1 = {
        Matrix:new{self[1], self[2]},
        Matrix:new{self[2], self[3]},
        Matrix:new{self[3], self[1]}
    }
    for _, e1 in ipairs(edges1) do 
        local A = e1:hline_segment_triangle_occlusion_sort(T)
        if A ~= nil then return A end
    end
    local points1 = {
        Vector:new(self[1]),
        Vector:new(self[2]),
        Vector:new(self[3])
    }
    for _, p1 in ipairs(points1) do 
        local A = p1:hpoint_triangle_occlusion_sort(T)
        if A ~= nil then return A end
    end
    local points2 = {
        Vector:new(T[1]),
        Vector:new(T[2]),
        Vector:new(T[3])
    }
    for _, p2 in ipairs(points2) do 
        local A = p2:hpoint_triangle_occlusion_sort(self)
        if A ~= nil then return not A end
    end
    return nil
end

--- Homogeneous occlusion sorting between two simplices
--- @param S1 table A table with 'type' and 'simplex' fields representing the first simplex
--- @param S2 table A table with 'type' and 'simplex' fields representing the second simplex
--- @return boolean|nil True if S1 is in front of S2, false if S2 is in front of S1
local function occlusion_sort_simplices(S1, S2)
    if S1.type == "point" and S2.type == "point" then 
        return S1.simplex:hpoint_point_occlusion_sort(S2.simplex)
    elseif S1.type == "point" and S2.type == "line segment" then 
        return S1.simplex:hpoint_line_segment_occlusion_sort(S2.simplex)
    elseif S1.type == "line segment" and S2.type == "point" then 
        local A = S2.simplex:hpoint_line_segment_occlusion_sort(S1.simplex)
        if A ~= nil then return not A end
    elseif S1.type == "point" and S2.type == "triangle" then 
        return S1.simplex:hpoint_triangle_occlusion_sort(S2.simplex)
    elseif S1.type == "triangle" and S2.type == "point" then 
        local A = S2.simplex:hpoint_triangle_occlusion_sort(S1.simplex)
        if A ~= nil then return not A end
    elseif S1.type == "line segment" and S2.type == "line segment" then 
        return S1.simplex:hline_segment_line_segment_occlusion_sort(S2.simplex)
    elseif S1.type == "line segment" and S2.type == "triangle" then 
        return S1.simplex:hline_segment_triangle_occlusion_sort(S2.simplex)
    elseif S1.type == "triangle" and S2.type == "line segment" then 
        local A = S2.simplex:hline_segment_triangle_occlusion_sort(S1.simplex)
        if A ~= nil then return not A end
    elseif S1.type == "triangle" and S2.type == "triangle" then
        return S1.simplex:htriangle_triangle_occlusion_sort(S2.simplex)
    end
end


--- Check if a line segment intersects a point in homogeneous coordinates
--- @param point Vector The point to check intersection with
--- @return boolean True if they intersect, false otherwise
function Matrix:hline_segment_point_intersecting(point)
    assert(
        getmetatable(self) == Matrix 
        and #self == 2
        and getmetatable(point) == Vector
        and #self[1] == #point,
        "Invalid arguments to hline_segment_point_intersecting"
    )
    local L = self:deep_copy() -- the line segment
    local P = point:deep_copy() -- the point
    local LA = L:hto_basis() -- the line segment in basis form
    local LO = Vector:new(LA[1]) -- the line segment origin
    local LU = Vector:new(LA[2]) -- the line segment direction
    local LOP = P:hsub(LO) -- vector from line origin to point
    local rhs = LOP:hproject_onto(LU) -- projected vector
    local orth = LO:hadd(rhs) -- orthogonal projection of point onto line

    -- Is the point coincident with it's orthogonal projection onto the line? 
    -- If not, no intersection.
    if not orth:hpoint_point_intersecting(P) then
        return false
    end
    
    local sol = Matrix:new{
        {LU[1], LU[2], LU[3]},
        {0, 0, 0},
        {0, 0, 0},
        {rhs[1], rhs[2], rhs[3]}
    }:column_reduction()
    if sol == nil then return false end
    -- print(sol)
    local t = sol[1]
    local eps = 1e-12
    -- being within the domain matters, 
    -- hence the direction of the eps checks
    if eps < t and t < 1 - eps then
        return true
    end
end


--- Partition a line segment by a point in homogeneous coordinates
--- @param point Vector The point to partition by
--- @return table|nil A table with two Matrix objects representing the partitioned line segments,
function Matrix:hpartition_line_segment_by_point(point)
    if self:hline_segment_point_intersecting(point) then
        return {
            Matrix:new{self[1], point:to_table()},
            Matrix:new{point:to_table(), self[2]}
        }
    end
    return nil
end

--- Compute the intersection of two lines in homogeneous coordinates
--- @param line Matrix A 2xN matrix representing the line in homogeneous coordinates
--- @return Vector The intersection point in homogeneous coordinates
function Matrix:hline_line_intersection(line)
    local basis1 = self:deep_copy()
    local basis2 = line:deep_copy()
    local O1 = Vector:new(basis1[1])
    local U1 = Vector:new(basis1[2])
    local O2 = Vector:new(basis2[1])
    local U2 = Vector:new(basis2[2])
    local rhs = O2:hsub(O1)
    local sol = Matrix:new{
        {U1[1], U1[2], U1[3]},
        {-U2[1], -U2[2], -U2[3]},
        {0, 0, 0},
        {rhs[1], rhs[2], rhs[3]}
    }:column_reduction()
    if sol == nil then return nil end
    -- if not parallel, then both will be finite, so we only check one
    return O1:hadd(U1:hscale(sol[1]))
end

--- Compute the intersection of two line segments in homogeneous coordinates
--- @param line Matrix A 2xN matrix representing the line segment in homogeneous coordinates
--- @return Vector|nil The intersection point in homogeneous coordinates, or nil if they don't intersect
function Matrix:hline_segment_line_segment_intersection(line)
    local L1 = self:deep_copy()
    local L2 = line:deep_copy()
    local L1A = L1:hto_basis()
    local L2A = L2:hto_basis()
    local I = L1A:hline_line_intersection(L2A)
    if I == nil then return nil end
    if L1:hline_segment_point_intersecting(I) 
    and L2:hline_segment_point_intersecting(I) then
        return I
    end
    return nil
end

--- Compute the intersection of a line and a plane in homogeneous coordinates
--- @param plane Matrix A 3xN matrix representing the plane in homogeneous coordinates
--- @return Vector The intersection point in homogeneous coordinates
function Matrix:hline_plane_intersection(plane)
    local L = self:deep_copy() -- the line (affine)
    local T = plane:deep_copy() -- the plane (affine)
    local LO = Vector:new(L[1]) -- the line origin
    local LU = Vector:new(L[2]) -- the line direction
    local TO = Vector:new(T[1]) -- the plane origin
    local TU = Vector:new(T[2]) -- the plane first direction
    local TV = Vector:new(T[3]) -- the plane second direction
    local rhs = TO:hsub(LO) -- vector from line origin to plane origin
    local sol = Matrix:new{
        {LU[1], LU[2], LU[3]},
        {-TU[1], -TU[2], -TU[3]},
        {-TV[1], -TV[2], -TV[3]},
        {rhs[1], rhs[2], rhs[3]}
    }:column_reduction()
    if sol == nil then return nil end
    return LO:hadd(LU:hscale(sol[1]))
end




--- Compute the intersection of two triangles in homogeneous coordinates
--- @param tri Matrix A 3xN matrix representing the triangle in homogeneous coordinates 
--- @return Matrix|nil A 2xN matrix representing the intersection line segment in homogeneous coordinates, or nil if they don't intersect
function Matrix:htriangle_triangle_intersections(tri)
    local T1 = self:deep_copy()
    local T2 = tri:deep_copy()
    local edges1 = {
        Matrix:new{T1[1], T1[2]},
        Matrix:new{T1[2], T1[3]},
        Matrix:new{T1[3], T1[1]}
    }
    local edges2 = {
        Matrix:new{T2[1], T2[2]},
        Matrix:new{T2[2], T2[3]},
        Matrix:new{T2[3], T2[1]}
    }
    local points = {}

    --- Check edges of T1 against T2
    --- @param edge Matrix The edge to check
    local function add_unique(point)
        for _, p in ipairs(points) do 
            if point:hpoint_point_intersecting(p) then
                return nil
            end
        end
        table.insert(points, point)
    end

    for _, edge in ipairs(edges1) do 
        local I = edge:hline_segment_triangle_intersection(T2)
        if I ~= nil then 
            add_unique(I)
        end
    end
    local f = false 
    if #points == 1 then 
        -- print("A) After checking edges of T1 against T2, found " .. #points .. " intersection points.")
        f = true
    end
    for _, edge in ipairs(edges2) do 
        local I = edge:hline_segment_triangle_intersection(T1)
        if I ~= nil then 
            add_unique(I)
        end
    end
    if f == true and #points == 2 then 
        -- print("B) After checking edges of T2 against T1, found " .. #points .. " intersection points.")
    end
    if #points == 0 then return nil end
    if #points ~= 2 then 
        return nil 
    end
    
    local ans = {}
    for _, p in ipairs(points) do 
        table.insert(ans, p:to_table())
    end
    return Matrix:new(ans)
end

--- Partition a line segment by another line segment in homogeneous coordinates
--- @param line Matrix A 2xN matrix representing the line segment to partition by
--- @return table|nil A table with two Matrix objects representing the partitioned line segments,
function Matrix:hpartition_line_segment_by_line_segment(line)
    local L1 = self:deep_copy()
    local L2 = line:deep_copy()
    local I = L1:hline_segment_line_segment_intersection(L2)
    if I ~= nil then
        return L1:hpartition_line_segment_by_point(I)
    end
    return nil
end

--- Compute the intersection of a line segment and a triangle in homogeneous coordinates
--- @param tri Matrix A 3xN matrix representing the triangle in homogeneous coordinates
--- @return Vector|nil The intersection point in homogeneous coordinates, or nil if they don't intersect
function Matrix:hline_segment_triangle_intersection(tri)
    local L = self:deep_copy() -- the line segment
    local T = tri:deep_copy() -- the triangle
    -- intersect basis
    local I = L:hto_basis():hline_plane_intersection(T:hto_basis())
    -- print(I)
    if I == nil then return nil end
    if L:hline_segment_point_intersecting(I) 
    and I:hpoint_triangle_intersecting(T) then
        return I
    end
    return nil
end

--- Partition a triangle by another triangle in homogeneous coordinates
--- @param tri Matrix A 3xN matrix representing the triangle to partition by
--- @return table|nil A table with three Matrix objects representing the partitioned triangles,
function Matrix:hpartition_triangle_by_triangle(tri)
    local T1 = self:deep_copy()
    local T2 = tri:deep_copy()
    local I = T1:htriangle_triangle_intersections(T2)
    if I == nil then return nil end 
    -- intersect basis
    local IA = I:hto_basis()
    local IO = Vector:new(IA[1])
    local IU = Vector:new(IA[2])
    -- triangle edges
    local edges1 = {
        Matrix:new{T1[1], T1[2]},
        Matrix:new{T1[2], T1[3]},
        Matrix:new{T1[3], T1[1]}
    }
    local edges2 = {
        Matrix:new{T2[1], T2[2]},
        Matrix:new{T2[2], T2[3]},
        Matrix:new{T2[3], T2[1]}
    }
    local bases1 = {}
    for i, edge in ipairs(edges1) do 
        bases1[i] = edge:hto_basis()
    end
    local bases2 = {}
    for i, edge in ipairs(edges2) do 
        bases2[i] = edge:hto_basis()
    end

    local points = {}
    

    --- Add unique point to points table
    --- @param point Vector The point to add
    local function add_point(point)
        for _, p in ipairs(points) do 
            if point:hpoint_point_intersecting(p) then
                return nil
            end
        end
        table.insert(points, point)
    end

    local non_intersecting = nil 
    for i, edge_basis in ipairs(bases1) do
        local int = IA:hline_line_intersection(edge_basis)
        if int ~= nil and edges1[i]:hline_segment_point_intersecting(int) then
            add_point(int)
        else 
            -- print("Edge " .. i .. " of triangle 1 does not intersect triangle 2.")
            non_intersecting = i
        end
    end
    -- print("Number of intersection points: " .. #points)
    if #points ~= 2 then return nil end -- doesn't get past here

    local quad = {}
    local tri1 
    local A, B = points[1], points[2]
    table.insert(quad, A)
    table.insert(quad, B)
    if non_intersecting == 1 then 
        table.insert(quad, edges1[1][1])
        table.insert(quad, edges1[1][2])
        tri1 = Matrix:new{A:to_table(), B:to_table(), edges1[2][2]}
    elseif non_intersecting == 2 then 
        table.insert(quad, edges1[2][1])
        table.insert(quad, edges1[2][2])
        tri1 = Matrix:new{A:to_table(), B:to_table(), edges1[3][2]}
    elseif non_intersecting == 3 then
        table.insert(quad, edges1[3][1])
        table.insert(quad, edges1[3][2])
        tri1 = Matrix:new{A:to_table(), B:to_table(), edges1[1][2]}
    else 
        -- print("Expected one non-intersecting edge, got none.")
    end
    quad = Matrix:new(quad):hcentroid_sort()
    local Q1 = Vector:new(quad[1])
    local Q2 = Vector:new(quad[2])
    local Q3 = Vector:new(quad[3])
    local Q4 = Vector:new(quad[4])
    if Q1:hdistance(Q3) > Q2:hdistance(Q4) then 
        return {
            tri1,
            Matrix:new{Q2:to_table(), Q1:to_table(), Q4:to_table()},
            Matrix:new{Q2:to_table(), Q3:to_table(), Q4:to_table()}
        }
    else
        return {
            tri1,
            Matrix:new{Q1:to_table(), Q2:to_table(), Q3:to_table()},
            Matrix:new{Q3:to_table(), Q4:to_table(), Q1:to_table()}
        }
    end
end










--- 3D y axis rotation matrix
--- @param theta number The rotation angle in radians
--- @return Matrix The rotation matrix
function Matrix.yrotation3(theta)
    local c = math.cos(theta)
    local s = math.sin(theta)
    return Matrix:new{
        { c, 0,  -s, 0},
        { 0, 1,  0, 0},
        { s, 0,  c, 0},
        { 0, 0,  0, 1}
    }
end

--- 3D translation matrix
--- @param dx number Translation along the x-axis
--- @param dy number Translation along the y-axis
--- @param dz number Translation along the z-axis
--- @return Matrix The translation matrix
function Matrix.translate3(dx, dy, dz)
    return Matrix:new{
        {1, 0, 0, 0},
        {0, 1, 0, 0},
        {0, 0, 1, 0},
        {dx, dy, dz, 1}
    }
end

--- 3D scaling matrix
--- @param sx number Scaling factor along the x-axis
--- @param sy number Scaling factor along the y-axis
--- @param sz number Scaling factor along the z-axis
--- @return Matrix The scaling matrix
function Matrix.scale3(sx, sy, sz)
    return Matrix:new{
        {sx, 0, 0, 0},
        {0, sy, 0, 0},
        {0, 0, sz, 0},
        {0, 0, 0, 1}
    }
end

--- 3D x axis rotation matrix
--- @param theta number The rotation angle in radians
--- @return Matrix The rotation matrix  
function Matrix.xrotation3(theta)
    local c = math.cos(theta)
    local s = math.sin(theta)
    return Matrix:new{
        {1, 0, 0, 0},
        {0, c, s, 0},
        {0, -s, c, 0},
        {0, 0, 0, 1}
    }
end

--- 3D z axis rotation matrix
--- @param theta number The rotation angle in radians
--- @return Matrix The rotation matrix
function Matrix.zrotation3(theta)
    local c = math.cos(theta)
    local s = math.sin(theta)
    return Matrix:new{
        { c, s, 0, 0},
        {-s, c, 0, 0},
        { 0, 0, 1, 0},
        { 0, 0, 0, 1}
    }
end

--- 3D ZYZ Euler rotation matrix
--- @param alpha number The first rotation angle in radians
--- @param beta number The second rotation angle in radians
--- @param gamma number The third rotation angle in radians
--- @return Matrix The rotation matrix
function Matrix.zyzrotation3(alpha, beta, gamma)
    return Matrix.zrotation3(gamma)
        :multiply(Matrix.yrotation3(beta))
        :multiply(Matrix.zrotation3(alpha))
end

--- 3D identity matrix
--- @return Matrix The identity matrix
function Matrix.identity3()
    return Matrix:new{
        {1, 0, 0, 0},
        {0, 1, 0, 0},
        {0, 0, 1, 0},
        {0, 0, 0, 1}
    }
end





















-- https://tex.stackexchange.com/a/747040
--- Creates a TeX command that evaluates a Lua function
---
--- @param name string The name of the `\csname` to define
--- @param func function
--- @param args table<string> The TeX types of the function arguments
--- @param protected boolean|nil Define the command as `\protected`
--- @return nil
local function register_tex_cmd(name, func, args, protected)
    -- The extended version of this function uses `N` and `w` where appropriate,
    -- but only using `n` is good enough for exposition purposes.
    name = "__lua_tikztdtools_" .. name .. ":" .. ("n"):rep(#args)

    -- Push the appropriate scanner functions onto the scanning stack.
    local scanners = {}
    for _, arg in ipairs(args) do
        scanners[#scanners+1] = token['scan_' .. arg]
    end

    -- An intermediate function that properly "scans" for its arguments
    -- in the TeX side.
    local scanning_func = function()
        local values = {}
        for _, scanner in ipairs(scanners) do
            values[#values+1] = scanner()
        end

        func(table.unpack(values))
    end

    local index = luatexbase.new_luafunction(name)
    lua.get_functions_table()[index] = scanning_func

    if protected then
        token.set_lua(name, index, "protected")
    else
        token.set_lua(name, index)
    end
end


--- Initialize the main table of the package.
local lua_tikz3dtools = {}
lua_tikz3dtools.simplices = {}
lua_tikz3dtools.math = {}
lua_tikz3dtools.lights = {}
for k, v in pairs(math) do lua_tikz3dtools.math[k] = v end
lua_tikz3dtools.math.Vector = Vector
-- for k, v in pairs(Vector) do lua_tikz3dtools.math.Vector[k] = v end
lua_tikz3dtools.math.Matrix = Matrix
-- for k, v in pairs(Matrix) do lua_tikz3dtools.math.Matrix[k] = v end


--- Evaluate a single string expression in the lua_tikz3dtools.math environment
--- @param str string The string expression
--- @return any The result of the expression
local function single_string_expression(str)
    return load(
        ("return %s"):format(str), 
        "expression", 
        "t", 
        lua_tikz3dtools.math
    )()
end


--- Evaluate a single string function in the lua_tikz3dtools.math environment
--- @param str string The string function
--- @return function The resulting function
local function single_string_function(str)
    return load(("return function(u) return %s end"):format(str), "expression", "t", lua_tikz3dtools.math)()
end

--- Evaluate a double string function in the lua_tikz3dtools.math environment
--- @param str string The string function
--- @return function The resulting function
local function double_string_function(str)
    return load(("return function(u,v) return %s end"):format(str), "expression", "t", lua_tikz3dtools.math)()
end

--- Evaluate a triple string function in the lua_tikz3dtools.math environment
--- @param str string The string function
--- @return function The resulting function
local function triple_string_function(str)
    return load(("return function(u,v,w) return %s end"):format(str), "expression", "t", lua_tikz3dtools.math)()
end

-- Append a point simplex to the global simplex list
--- @param hash table A table with keys 'x', 'y', 'z', 'transformation', 'filloptions', and 'filter'
local function append_point(hash)
    local x              = single_string_expression(hash.x)
    local y              = single_string_expression(hash.y)
    local z              = single_string_expression(hash.z)
    local transformation = single_string_expression(hash.transformation)
    local filloptions    = hash.filloptions
    local filter         = hash.filter
    if x and y and z then 
        local A =  Vector:new{x, y, z, 1} 
        local the_simplex = A:multiply(transformation)
        table.insert(
            lua_tikz3dtools.simplices, 
            { 
                simplex     = the_simplex, 
                filloptions = filloptions, 
                type        = "point",
                filter      = filter
            }
        )
    end
end
register_tex_cmd(
    "appendpoint",
    function()
        append_point{
            x              = token.get_macro("luatikztdtools@p@p@x"),
            y              = token.get_macro("luatikztdtools@p@p@y"),
            z              = token.get_macro("luatikztdtools@p@p@z"),
            filloptions    = token.get_macro("luatikztdtools@p@p@filloptions"),
            transformation = token.get_macro("luatikztdtools@p@p@transformation"),
            filter         = token.get_macro("luatikztdtools@p@p@filter")
        } 
    end,
    { }
)

-- Append a parametric surface to the global simplex list
--- @param hash table A table with keys 'ustart', 'ustop', 'usamples', 'vstart', 'vstop', 'vsamples
local function append_surface(hash)
    local ustart         = single_string_expression(hash.ustart)
    local ustop          = single_string_expression(hash.ustop)
    local usamples       = single_string_expression(hash.usamples)
    local vstart         = single_string_expression(hash.vstart)
    local vstop          = single_string_expression(hash.vstop)
    local vsamples       = single_string_expression(hash.vsamples)
    local transformation = single_string_expression(hash.transformation)
    local x              = double_string_function(hash.x)
    local y              = double_string_function(hash.y)
    local z              = double_string_function(hash.z)
    local filloptions    = hash.filloptions
    local filter         = hash.filter
    local ustep = (ustop - ustart) / (usamples - 1)
    local vstep = (vstop - vstart) / (vsamples - 1)

    --- Parametric surface function
    --- @param u number The u parameter
    --- @param v number The v parameter
    --- @return Vector|nil The point on the surface as a Vector, or nil if
    local function parametric_surface(u, v)
        local px, py, pz = x(u, v), y(u, v), z(u, v)
        if px and py and pz then 
            return Vector:new{px, py, pz, 1}
        else
            return nil
        end
    end
    
    for i = 0, usamples - 2 do 
        local u = ustart + i * ustep 
        for j = 0, vsamples - 2 do 
            local v = vstart + j * vstep
            local A = parametric_surface(u, v)
            local B = parametric_surface(u + ustep, v)
            local C = parametric_surface(u + ustep, v + vstep)
            local D = parametric_surface(u, v + vstep)
            if A and B and C and D then 
                local simplex1 = Matrix:new{A:to_table(), B:to_table(), D:to_table()}:multiply(transformation)
                local simplex2 = Matrix:new{B:to_table(), C:to_table(), D:to_table()}:multiply(transformation)
                if not (
                    A:hpoint_point_intersecting(B)
                    or B:hpoint_point_intersecting(D)
                    or A:hpoint_point_intersecting(D)
                ) then
                    table.insert(
                        lua_tikz3dtools.simplices, 
                        { 
                            simplex     = simplex1, 
                            filloptions = filloptions, 
                            type        = "triangle",
                            filter      = filter
                        }
                    )
                end
                if not (
                    A:hpoint_point_intersecting(C)
                    or C:hpoint_point_intersecting(D)
                    or A:hpoint_point_intersecting(D)
                ) then 
                    table.insert(
                        lua_tikz3dtools.simplices, 
                        { 
                            simplex     = simplex2, 
                            filloptions = filloptions, 
                            type        = "triangle",
                            filter      = filter
                        }
                    )
                end
            end
        end
    end
end
register_tex_cmd(
    "appendsurface", 
    function()
        append_surface{
            ustart         = token.get_macro("luatikztdtools@p@s@ustart"),
            ustop          = token.get_macro("luatikztdtools@p@s@ustop"),
            usamples       = token.get_macro("luatikztdtools@p@s@usamples"),
            vstart         = token.get_macro("luatikztdtools@p@s@vstart"),
            vstop          = token.get_macro("luatikztdtools@p@s@vstop"),
            vsamples       = token.get_macro("luatikztdtools@p@s@vsamples"),
            x              = token.get_macro("luatikztdtools@p@s@x"),
            y              = token.get_macro("luatikztdtools@p@s@y"),
            z              = token.get_macro("luatikztdtools@p@s@z"),
            transformation = token.get_macro("luatikztdtools@p@s@transformation"),
            filloptions    = token.get_macro("luatikztdtools@p@s@filloptions"),
            filter         = token.get_macro("luatikztdtools@p@s@filter"),
        } 
    end,
    { }
)

-- Append a triangle simplex to the global simplex list
--- @param hash table A table with keys 'A', 'B', 'C', '
local function append_triangle(hash)
    local A              = single_string_expression(hash.A)
    local B              = single_string_expression(hash.B)
    local C              = single_string_expression(hash.C)
    local transformation = single_string_expression(hash.transformation)
    local filter         = hash.filter
    local filloptions    = hash.filloptions

    if A[1][1] and B[1][1] and C[1][1] then
        if not (
            P:hpoint_point_intersecting(Q)
            or Q:hpoint_point_intersecting(R)
            or R:hpoint_point_intersecting(P)
        ) then 
            local the_simplex = Matrix:new{A[1], B[1], C[1]}:multiply(transformation)
            table.insert(
                lua_tikz3dtools.simplices, 
                { 
                    simplex     = the_simplex, 
                    filloptions = filloptions, 
                    type        = "triangle",
                    filter      = filter
                }
            )
        end
    end
end
register_tex_cmd(
    "appendtriangle", 
    function()
        append_triangle{
            A = token.get_macro("luatikztdtools@p@t@A"),
            B = token.get_macro("luatikztdtools@p@t@B"),
            C = token.get_macro("luatikztdtools@p@t@C"),
            transformation = token.get_macro("luatikztdtools@p@t@transformation"),
            filloptions    = token.get_macro("luatikztdtools@p@t@filloptions"),
            filter         = token.get_macro("luatikztdtools@p@t@filter"),
        } 
    end,
    { }
)

-- Append a label simplex to the global simplex list
--- @param hash table A table with keys 'x', 'y', 'z', '
local function append_label(hash)
    local x              = single_string_expression(hash.x)
    local y              = single_string_expression(hash.y)
    local z              = single_string_expression(hash.z)
    local filter         = hash.filter
    local text           = hash.text
    local transformation = single_string_expression(hash.transformation)
    if x and y and z then 
        local A =  Vector:new{x, y, z, 1} 
        local the_simplex = A:multiply(transformation)
        table.insert(
            lua_tikz3dtools.simplices, 
            { 
                simplex     = the_simplex, 
                text        = text,
                type        = "label",
                filter      = filter
            }
        )
    end
end
register_tex_cmd(
    "appendlabel",
    function()
        append_label{
            x              = token.get_macro("luatikztdtools@p@l@x"),
            y              = token.get_macro("luatikztdtools@p@l@y"),
            z              = token.get_macro("luatikztdtools@p@l@z"),
            text           = token.get_macro("luatikztdtools@p@l@text"),
            transformation = token.get_macro("luatikztdtools@p@l@transformation"),
            filter         = token.get_macro("luatikztdtools@p@l@filter")
        } 
    end,
    { }
)

local function append_light(hash)
    local x = single_string_expression(hash.x)
    local y = single_string_expression(hash.y)
    local z = single_string_expression(hash.z)
    if x and y and z then 
        local vec = Vector:new{x, y, z, 1}
        table.insert(
            lua_tikz3dtools.lights,
            vec
        )
    end
end
register_tex_cmd(
    "appendlight",
    function()
        append_light{
            x = token.get_macro("luatikztdtools@p@l@x"),
            y = token.get_macro("luatikztdtools@p@l@y"),
            z = token.get_macro("luatikztdtools@p@l@z")
        } 
    end,
    { }
)

--- Append a parametric curve to the global simplex list
--- @param hash table A table with keys 'ustart', 'ustop', 'us
local function append_curve(hash)
    local ustart         = single_string_expression(hash.ustart)
    local ustop          = single_string_expression(hash.ustop)
    local usamples       = single_string_expression(hash.usamples)
    local transformation = single_string_expression(hash.transformation)
    local x              = single_string_function(hash.x)
    local y              = single_string_function(hash.y)
    local z              = single_string_function(hash.z)
    local filter         = hash.filter
    local drawoptions    = hash.drawoptions
    local arrowoptions   = hash.arrowoptions
    local tailoptions    = hash.tailoptions
    local ustep = (ustop - ustart) / (usamples - 1)

    --- Parametric curve function
    --- @param u number The u parameter
    --- @return Vector|nil The point on the curve as a Vector, or nil if
    local function parametric_curve(u)
        local px, py, pz = x(u), y(u), z(u)
        if px and py and pz then 
            return Vector:new{px, py, pz, 1}
        else
            return nil
        end
    end
    for i = 0, usamples - 2 do 
        local u = ustart + i * ustep
        local A = parametric_curve(u)
        local B = parametric_curve(u + ustep)
        if A and B then 
            local simplex = Matrix:new{A:to_table(), B:to_table()}:multiply(transformation)
            table.insert(
                lua_tikz3dtools.simplices, 
                { 
                    simplex      = simplex, 
                    drawoptions  = drawoptions,
                    type         = "line segment",
                    filter       = filter
                }
            )
            if i == 0 and tailoptions then 
                --
            elseif i == usamples - 2 and arrowoptions then 
                local P = parametric_curve(ustop)
                local NP = parametric_curve(ustop - ustep):hsub(NP):hnormalize()
                local U = NP:hhypercross(NP:orthogonal_vector())
                local V = NP:hhypercross(U)
                append_surface{
                    ustart = 0
                    ,ustop = 0.1
                    ,usamples = 2
                    ,vstart = 0
                    ,vstop = 1
                    ,vsamples = 4
                    ,x = "u*cos(v*tau)"
                    ,y = "u*sin(v*tau)"
                    ,z = "-u"
                    ,filloptions = arrowoptions
                    ,transformation = ([[
                        multiply(
                            {
                                {%f,%f,%f,0}
                                ,{%f,%f,%f,0}
                                ,{%f,%f,%f,0}
                                ,{%f,%f,%f,1}
                            }
                            ,%s
                        )
                    ]]):format(
                        U[1][1],U[1][2],U[1][3]
                        ,V[1][1],V[1][2],V[1][3]
                        ,NP[1][1],NP[1][2],NP[1][3]
                        ,P[1][1],P[1][2],P[1][3]
                        ,hash.transformation
                    )
                    ,filter = filter
                }
            end
        end
    end
end
register_tex_cmd(
    "appendcurve",
    function()
        append_curve{
            ustart         = token.get_macro("luatikztdtools@p@c@ustart"),
            ustop          = token.get_macro("luatikztdtools@p@c@ustop"),
            usamples       = token.get_macro("luatikztdtools@p@c@usamples"),
            x              = token.get_macro("luatikztdtools@p@c@x"),
            y              = token.get_macro("luatikztdtools@p@c@y"),
            z              = token.get_macro("luatikztdtools@p@c@z"),
            transformation = token.get_macro("luatikztdtools@p@c@transformation"),
            drawoptions    = token.get_macro("luatikztdtools@p@c@drawoptions"),
            arrowtip       = token.get_macro("luatikztdtools@p@c@arrowtip"),
            arrowtail      = token.get_macro("luatikztdtools@p@c@arrowtail"),
            arrowoptions   = token.get_macro("luatikztdtools@p@c@arrowtipoptions"),
            filter         = token.get_macro("luatikztdtools@p@c@filter")
        } 
    end,
    { }
)


-- Append a parametric solid to the global simplex list
--- @param hash table A table with keys 'ustart', 'ustop', 'us
local function append_solid(hash)
    local ustart   = single_string_expression(hash.ustart)
    local ustop    = single_string_expression(hash.ustop)
    local usamples = single_string_expression(hash.usamples)
    local vstart   = single_string_expression(hash.vstart)
    local vstop    = single_string_expression(hash.vstop)
    local vsamples = single_string_expression(hash.vsamples)
    local wstart   = single_string_expression(hash.wstart)
    local wstop    = single_string_expression(hash.wstop)
    local wsamples = single_string_expression(hash.wsamples)
    local filloptions    = hash.filloptions
    local transformation = single_string_expression(hash.transformation)
    local filter = hash.filter
    local x = triple_string_function(hash.x)
    local y = triple_string_function(hash.y)
    local z = triple_string_function(hash.z)

    --- Parametric solid function
    --- @param u number The u parameter
    --- @param v number The v parameter
    --- @param w number The w parameter
    --- @return Vector|nil The point in the solid as a Vector, or nil if
    local function parametric_solid(u, v, w)
        local px, py, pz = x(u, v, w), y(u, v, w), z(u, v, w)
        if px and py and pz then 
            return Vector:new{px, py, pz, 1}
        else
            return nil
        end
    end

    local ustep = (ustop - ustart) / (usamples - 1)
    local vstep = (vstop - vstart) / (vsamples - 1)
    local wstep = (wstop - wstart) / (wsamples - 1)

    --- Tessellate a face of the solid
    --- @param fixed_var string The fixed parameter variable ("u", "v", or "w")
    --- @param fixed_val number The fixed parameter value
    --- @param s1_start number The start value of the first varying parameter
    --- @param s1_step number The step size of the first varying parameter
    --- @param s1_count number The number of samples of the first varying parameter
    --- @param s2_start number The start value of the second varying parameter
    --- @param s2_step number The step size of the second varying parameter
    --- @param s2_count number The number of samples of the second varying parameter
    local function tessellate_face(fixed_var, fixed_val, s1_start, s1_step, s1_count, s2_start, s2_step, s2_count)
        for i = 0, s1_count - 2 do 
            local s1 = s1_start + i * s1_step
            for j = 0, s2_count - 2 do 
                local s2 = s2_start + j * s2_step
                local A, B, C, D
                if fixed_var == "u" then 
                    A = parametric_solid(fixed_val, s1, s2)
                    B = parametric_solid(fixed_val, s1 + s1_step, s2)
                    C = parametric_solid(fixed_val, s1 + s1_step, s2 + s2_step)
                    D = parametric_solid(fixed_val, s1, s2 + s2_step)
                elseif fixed_var == "v" then 
                    A = parametric_solid(s1, fixed_val, s2)
                    B = parametric_solid(s1 + s1_step, fixed_val, s2)
                    C = parametric_solid(s1 + s1_step, fixed_val, s2 + s2_step)
                    D = parametric_solid(s1, fixed_val, s2 + s2_step)
                elseif fixed_var == "w" then 
                    A = parametric_solid(s1, s2, fixed_val)
                    B = parametric_solid(s1 + s1_step, s2, fixed_val)
                    C = parametric_solid(s1 + s1_step, s2 + s2_step, fixed_val)
                    D = parametric_solid(s1, s2 + s2_step, fixed_val)
                end
                if A and B and D then 
                    local simplex = Matrix:new{A:to_table(), B:to_table(), D:to_table()}:multiply(transformation)
                    table.insert(
                        lua_tikz3dtools.simplices, 
                        { 
                            simplex     = simplex, 
                            filloptions = filloptions, 
                            type        = "triangle",
                            filter      = filter
                        }
                    )
                end 
                if A and C and D then 
                    local simplex = Matrix:new{B:to_table(), C:to_table(), D:to_table()}:multiply(transformation)
                    table.insert(
                        lua_tikz3dtools.simplices, 
                        { 
                            simplex     = simplex, 
                            filloptions = filloptions, 
                            type        = "triangle",
                            filter      = filter
                        }
                    )
                end
            end
        end
    end

    tessellate_face("u", ustart, vstart, vstep, vsamples, wstart, wstep, wsamples)
    tessellate_face("u", ustop, vstart, vstep, vsamples, wstart, wstep, wsamples)
    tessellate_face("v", vstart, ustart, ustep, usamples, wstart, wstep, wsamples)
    tessellate_face("v", vstop, ustart, ustep, usamples, wstart, wstep, wsamples)
    tessellate_face("w", wstart, ustart, ustep, usamples, vstart, vstep, vsamples)
    tessellate_face("w", wstop, ustart, ustep, usamples, vstart, vstep, vsamples)
end
register_tex_cmd(
    "appendsolid", 
    function()
        append_solid{
            ustart         = token.get_macro("luatikztdtools@p@solid@ustart"),
            ustop          = token.get_macro("luatikztdtools@p@solid@ustop"),
            usamples       = token.get_macro("luatikztdtools@p@solid@usamples"),
            vstart         = token.get_macro("luatikztdtools@p@solid@vstart"),
            vstop          = token.get_macro("luatikztdtools@p@solid@vstop"),
            vsamples       = token.get_macro("luatikztdtools@p@solid@vsamples"),
            wstart         = token.get_macro("luatikztdtools@p@solid@wstart"),
            wstop          = token.get_macro("luatikztdtools@p@solid@wstop"),
            wsamples       = token.get_macro("luatikztdtools@p@solid@wsamples"),
            x              = token.get_macro("luatikztdtools@p@solid@x"),
            y              = token.get_macro("luatikztdtools@p@solid@y"),
            z              = token.get_macro("luatikztdtools@p@solid@z"),
            transformation = token.get_macro("luatikztdtools@p@solid@transformation"),
            filloptions    = token.get_macro("luatikztdtools@p@solid@filloptions"),
            filter         = token.get_macro("luatikztdtools@p@solid@filter")
        } 
    end,
    { }
)

--- Apply filters to a list of simplices
--- @param simplices table A list of simplices
--- @return table A new list of simplices after applying the filters
local function apply_filters(simplices)
    local new_simplices = {}
    for i, simplex in ipairs(simplices) do
        if simplex.type == "point" then 
            local A = Vector:new(simplex.simplex:to_table())
            local env = { A = A }
            for k, v in pairs(lua_tikz3dtools.math) do 
                env[k] = v
            end
            local chunk, load_err = load(
                ("return %s"):format(simplex.filter or "true"),
                "filter",
                "t",
                env
            )
            local ok, result = pcall(chunk)
            if result then 
                table.insert(new_simplices, simplex)
            end
        elseif simplex.type == "line segment" then 
            local A = Vector:new(simplex.simplex[1])
            local B = Vector:new(simplex.simplex[2])
            local env = { A = A, B = B }
            for k, v in pairs(lua_tikz3dtools.math) do 
                env[k] = v
            end
            local chunk, load_err = load(
                ("return %s"):format(simplex.filter),
                "filter",
                "t",
                env
            )
            local ok, result = pcall(chunk)
            if result then 
                table.insert(new_simplices, simplex)
            end
        elseif simplex.type == "triangle" then 
            local A = Vector:new(simplex.simplex[1])
            local B = Vector:new(simplex.simplex[2])
            local C = Vector:new(simplex.simplex[3])
            local env = { A = A, B = B, C = C }
            for k, v in pairs(lua_tikz3dtools.math) do 
                env[k] = v
            end
            local chunk, load_err = load(
                ("return %s"):format(simplex.filter),
                "filter",
                "t",
                env
            )
            local ok, result = pcall(chunk)
            if result then 
                table.insert(new_simplices, simplex)
            end
        elseif simplex.type == "label" then 
            local A = Vector:new(simplex.simplex:to_table())
            local env = { A = A }
            for k, v in pairs(lua_tikz3dtools.math) do 
                env[k] = v
            end
            local chunk, load_err = load(
                ("return %s"):format(simplex.filter or "true"),
                "filter",
                "t",
                env
            )
            local ok, result = pcall(chunk)
            if result then 
                table.insert(new_simplices, simplex)
            end
        end
    end
    return new_simplices 
end


--- Strongly connected components sort for simplices based on occlusion
--- @param simplices table A list of simplices
--- @return table A new list of simplices sorted by occlusion
local function scc(simplices)
    -- Build adjacency list and in-degree count
    local n = #simplices
    local adj = {}
    local indegree = {}
    for i = 1, n do
        adj[i] = {}
        indegree[i] = 0
    end

    -- Build the directed graph: edge from i to j if i occludes j
    for i = 1, n do
        for j = 1, n do
            if i ~= j and j > i then
                local cmp = occlusion_sort_simplices(simplices[i], simplices[j])
                if cmp == true then
                    -- i occludes j
                    table.insert(adj[i], j)
                    indegree[j] = indegree[j] + 1
                elseif cmp == false then
                    -- j occludes i (reverse the edge)
                    table.insert(adj[j], i)
                    indegree[i] = indegree[i] + 1
                elseif cmp == nil then
                    -- Inconclusive: no edge is added
                end
            end
        end
    end

    -- Kahn's algorithm for topological sort
    local queue = {}
    for i = 1, n do
        if indegree[i] == 0 then
            table.insert(queue, i)
        end
    end

    local sorted = {}
    while #queue > 0 do
        local u = table.remove(queue, 1)
        table.insert(sorted, simplices[u])
        for _, v in ipairs(adj[u]) do
            indegree[v] = indegree[v] - 1
            if indegree[v] == 0 then
                table.insert(queue, v)
            end
        end
    end
    return sorted
end

--- Partition simplices by their parents, recursively, retaining all terminal pieces.
--- @param simplices table A list of simplex tables
--- @param parents table A list of parent simplex tables
--- @return table A flat list of all terminal partitioned simplices
local function partition_simplices_by_parents(simplices, parents)
    local result = {}

    -- Recursive helper
    local function partition_recursive(piece, parent_idx)
        if parent_idx > #parents then
            table.insert(result, piece)
            return
        end

        local parent = parents[parent_idx]
        local meta = {}
        for k, v in pairs(piece) do
            if k ~= "simplex" and k ~= "type" then
                meta[k] = v
            end
        end

        local parts = nil
        if parent.type == "point" and piece.type == "line segment" then
            parts = piece.simplex:hpartition_line_segment_by_point(parent.simplex)
            if parts then
                for _, part in ipairs(parts) do
                    local new_piece = { simplex = part, type = "line segment" }
                    for k, v in pairs(meta) do new_piece[k] = v end
                    partition_recursive(new_piece, parent_idx + 1)
                end
                return
            end
        elseif parent.type == "line segment" and piece.type == "line segment" then
            parts = piece.simplex:hpartition_line_segment_by_line_segment(parent.simplex)
            if parts then
                for _, part in ipairs(parts) do
                    local new_piece = { simplex = part, type = "line segment" }
                    for k, v in pairs(meta) do new_piece[k] = v end
                    partition_recursive(new_piece, parent_idx + 1)
                end
                return
            end
        elseif parent.type == "triangle" and piece.type == "line segment" then
            parts = piece.simplex:hpartition_line_segment_by_triangle(parent.simplex)
            if parts then
                for _, part in ipairs(parts) do
                    local new_piece = { simplex = part, type = "line segment" }
                    for k, v in pairs(meta) do new_piece[k] = v end
                    partition_recursive(new_piece, parent_idx + 1)
                end
                return
            end
        elseif parent.type == "triangle" and piece.type == "triangle" then
            parts = piece.simplex:hpartition_triangle_by_triangle(parent.simplex)
            if parts then
                for _, part in ipairs(parts) do
                    local new_piece = { simplex = part, type = "triangle" }
                    for k, v in pairs(meta) do new_piece[k] = v end
                    partition_recursive(new_piece, parent_idx + 1)
                end
                return
            end
        end

        -- If not partitioned, continue with next parent
        partition_recursive(piece, parent_idx + 1)
    end

    for _, simplex in ipairs(simplices) do
        partition_recursive(simplex, 1)
    end

    return result
end





--- Display the simplices in the global simplex list by outputting TikZ commands
local function display_simplices()
    global_seen = {}
    lua_tikz3dtools.simplices = partition_simplices_by_parents(
        lua_tikz3dtools.simplices,
        lua_tikz3dtools.simplices
    )
    lua_tikz3dtools.simplices = apply_filters(lua_tikz3dtools.simplices)
    lua_tikz3dtools.simplices = scc(lua_tikz3dtools.simplices)
    local labels = {}
    for _, simplex in ipairs(lua_tikz3dtools.simplices) do 
        if simplex.type == "point" then 
            tex.sprint(
                ("\\path[%s] (%f,%f) circle[radius = 0.06];")
                :format(
                    simplex.filloptions,
                    simplex.simplex[1], simplex.simplex[2]
                )
            )
        elseif simplex.type == "line segment" then 
            tex.sprint(
                ("\\path[%s] (%f,%f) -- (%f,%f);")
                :format(
                    simplex.drawoptions,
                    simplex.simplex[1][1], simplex.simplex[1][2],
                    simplex.simplex[2][1], simplex.simplex[2][2]
                )
            )
        elseif simplex.type == "triangle" then 
            local total_intensity = 0
            local num_lights = #lua_tikz3dtools.lights
            if num_lights > 0 then
                local A = Vector:new(simplex.simplex[1])
                local B = Vector:new(simplex.simplex[2])
                local C = Vector:new(simplex.simplex[3])
                local normal = (B:hsub(A)):hhypercross(C:hsub(A)):hnormalize()
                for _, light in ipairs(lua_tikz3dtools.lights) do
                    local light_dir = (light:hsub(A)):hnormalize()
                    total_intensity = total_intensity + math.abs(normal:hinner(light_dir))
                end
                local avg_intensity = (total_intensity / num_lights) * 100
                avg_intensity = math.floor(avg_intensity + 0.01)
                tex.sprint(("\\colorlet{brightness}{white!%f!black}"):format(avg_intensity))
            else
                tex.sprint(("\\colorlet{brightness}{white!%f!black}"):format(0))
            end
            tex.sprint(
                ("\\path[%s] (%f,%f) -- (%f,%f) -- (%f,%f) -- cycle;")
                :format(
                    simplex.filloptions,
                    simplex.simplex[1][1], simplex.simplex[1][2],
                    simplex.simplex[2][1], simplex.simplex[2][2],
                    simplex.simplex[3][1], simplex.simplex[3][2]
                )
            )
        elseif simplex.type == "label" then 
            table.insert(labels, simplex)
        end
    end
    for _, simplex in ipairs(labels) do 
        if simplex.type == "label" then 
            tex.sprint(
                ("\\node at (%f,%f) {%s};")
                :format(
                    simplex.simplex[1], simplex.simplex[2],
                    simplex.text
                )
            )
        end
    end
    lua_tikz3dtools.simplices = {}
end
register_tex_cmd("displaysimplices", function() display_simplices() end, { })

--- Create an object table from a name and a table
--- @param name string The name of the object
--- @param tbl table The table representing the object
--- @return table The object table
local function make_object(name, tbl)
    local obj = {}
    obj[name] = tbl
    return obj
end

--- Set a mathematical object in the global math object table
--- @param hash table A table with keys 'name' and 'object' 
local function set_object(hash)
    local object = single_string_expression(hash.object)
    local name = hash.name
    local tbl = make_object(name, object)
    for k, v in pairs(tbl) do 
        lua_tikz3dtools.math[k] = v
    end
end
register_tex_cmd(
    "setobject", 
    function()
        set_object{
            name   = token.get_macro("luatikztdtools@p@m@name"),
            object = token.get_macro("luatikztdtools@p@m@object"),
        } 
    end,
    { }
)