--- lua-tikz3dtools-geometry.lua
--- Computational geometry utilities for the lua-tikz3dtools package.
--- Intersection tests, occlusion sorting, partitioning, and spatial indexing.

local Vector
local Matrix

local Geometry = {}

--- Set class references (called after all modules are loaded).
function Geometry._set_classes(V, M)
    Vector = V
    Matrix = M
end

-- ================================================================
-- Point-level geometry (attached to Vector via methods below)
-- ================================================================

--- Check if two homogeneous vectors intersect (are the same point)
--- @param self Vector
--- @param other Vector
--- @return boolean
function Geometry.hpoint_point_intersecting(self, other)
    return self:hdistance(other) < 1e-12
end

--- Check if a homogeneous point is in the triangular prism defined by triangle T
--- @param self Vector
--- @param T Matrix
--- @return boolean
function Geometry.hpoint_in_triangular_prism(self, 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 self Vector
--- @param T Matrix
--- @return boolean
function Geometry.hpoint_triangle_intersecting(self, T)
    local Tbasis = T:deep_copy():hto_basis()
    local S = self:horthogonal_projection_onto_plane(Tbasis)
    return Geometry.hpoint_in_triangular_prism(S, T)
       and Geometry.hpoint_point_intersecting(self, S)
end

--- Check occlusion sort of a homogeneous point with a homogeneous point
--- @param self Vector
--- @param P Vector
--- @return boolean|nil
function Geometry.hpoint_point_occlusion_sort(self, P)
    if not Geometry.hpoint_point_intersecting(self, P) then
        if Geometry.hpoint_point_intersecting(
            Vector:_new{self[1], self[2], 0, 1},
            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 self Vector
--- @param other Vector
--- @return boolean
function Geometry.hcollinear(self, other)
    local cross = self:hhypercross(other)
    return cross:hnorm() < 1e-12
end

--- Check opposite direction of two homogeneous vectors
--- @param self Vector
--- @param other Vector
--- @return boolean
function Geometry.hoppositely_directed(self, other)
    return self:hinner(other) < 0
end

--- Check occlusion sort of a homogeneous point with a homogeneous line segment
--- @param self Vector
--- @param L Matrix
--- @return boolean|nil
function Geometry.hpoint_line_segment_occlusion_sort(self, L)
    local LA = L:hto_basis()
    local LO = Vector:_new(LA[1])
    local OP = self:hsub(LO)
    local LU = Vector:_new(LA[2])
    local proj = OP:hproject_onto(LU)
    local true_proj = LO:hadd(proj)
    local F = Vector:_new{self[1], self[2], 0, 1}
    local G = Vector:_new{true_proj[1], true_proj[2], 0, 1}
    if Geometry.hpoint_point_intersecting(F, G) then
        local temp = 1
        if Geometry.hoppositely_directed(proj, LU) then
            temp = -1
        end
        local test = temp * proj:hnorm() / LU:hnorm()
        local eps = 1e-12
        if (eps < test and test < 1 - eps) then
            return Geometry.hpoint_point_occlusion_sort(self, true_proj)
        end
    end
    return nil
end

--- Check occlusion sort of a homogeneous point with a homogeneous triangle
--- @param self Vector
--- @param T Matrix
--- @return boolean|nil
function Geometry.hpoint_triangle_occlusion_sort(self, 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 Geometry.hpoint_point_intersecting(self, Ta) then return nil end
    if Geometry.hpoint_point_intersecting(self, Tb) then return nil end
    if Geometry.hpoint_point_intersecting(self, Tc) then return nil end
    local eps = 1e-12
    if Geometry.hpoint_in_triangular_prism(P1, 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 Geometry.hpoint_point_occlusion_sort(self, a)
        else
            return nil
        end
    end
    return nil
end

-- ================================================================
-- Line-segment level geometry (attached to Matrix via methods below)
-- ================================================================

--- Unified line-segment / point intersection test.
--- @param self Matrix  2-row line segment
--- @param point Vector The point to test
--- @param boundary_inclusive boolean If true, endpoints count as intersecting
--- @return boolean
function Geometry.hline_segment_point_intersecting(self, point, boundary_inclusive)
    local LA = self:hto_basis()
    local LO = Vector:_new(LA[1])
    local LU = Vector:_new(LA[2])
    local LOP = point:hsub(LO)
    local rhs = LOP:hproject_onto(LU)
    local orth = LO:hadd(rhs)

    if not Geometry.hpoint_point_intersecting(orth, point) 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
    local t = sol[1]

    local eps = 1e-12
    if boundary_inclusive then
        return -eps < t and t < 1 + eps
    else
        return eps < t and t < 1 - eps
    end
end

--- Partition a line segment by a point
--- @param self Matrix
--- @param point Vector
--- @return table|nil
function Geometry.hpartition_line_segment_by_point(self, point)
    if Geometry.hline_segment_point_intersecting(self, point, false) 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 (basis form)
--- @param self Matrix
--- @param line Matrix
--- @return Vector|nil
function Geometry.hline_line_intersection(self, line)
    local O1 = Vector:_new(self[1])
    local U1 = Vector:_new(self[2])
    local O2 = Vector:_new(line[1])
    local U2 = Vector:_new(line[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
    return O1:hadd(U1:hscale(sol[1]))
end

--- Compute the intersection of two line segments
--- @param self Matrix
--- @param line Matrix
--- @return Vector|nil
function Geometry.hline_segment_line_segment_intersection(self, line)
    local L1A = self:hto_basis()
    local L2A = line:hto_basis()
    local I = Geometry.hline_line_intersection(L1A, L2A)
    if I == nil then return nil end
    if Geometry.hline_segment_point_intersecting(self, I, false)
    and Geometry.hline_segment_point_intersecting(line, I, false) then
        return I
    end
    return nil
end

--- Compute the intersection of a line and a plane (both in basis form)
--- @param self Matrix line basis
--- @param plane Matrix plane basis
--- @return Vector|nil
function Geometry.hline_plane_intersection(self, plane)
    local LO = Vector:_new(self[1])
    local LU = Vector:_new(self[2])
    local TO = Vector:_new(plane[1])
    local TU = Vector:_new(plane[2])
    local TV = Vector:_new(plane[3])
    local rhs = TO:hsub(LO)
    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 a line segment and a triangle
--- @param self Matrix line segment (2 rows)
--- @param tri Matrix triangle (3 rows)
--- @return Vector|nil
function Geometry.hline_segment_triangle_intersection(self, tri)
    -- skip if any endpoints coincide with triangle vertices
    for _, P1 in ipairs(self) do
        for _, P2 in ipairs(tri) do
            if Geometry.hpoint_point_intersecting(Vector:_new(P1), Vector:_new(P2)) then
                return nil
            end
        end
    end
    local I = Geometry.hline_plane_intersection(self:hto_basis(), tri:hto_basis())
    if I == nil then return nil end
    if not Geometry.hline_segment_point_intersecting(self, I, true) then
        return nil
    end
    if not Geometry.hpoint_triangle_intersecting(I, tri) then
        return nil
    end
    return I
end

--- Compute the intersection of two triangles
--- @param self Matrix
--- @param tri Matrix
--- @return Matrix|nil  2-row intersection segment, or nil
function Geometry.htriangle_triangle_intersections(self, tri)
    local edges1 = {
        Matrix:_new{self[1], self[2]},
        Matrix:_new{self[2], self[3]},
        Matrix:_new{self[3], self[1]}
    }
    local edges2 = {
        Matrix:_new{tri[1], tri[2]},
        Matrix:_new{tri[2], tri[3]},
        Matrix:_new{tri[3], tri[1]}
    }
    local points = {}

    local function add_unique(point)
        for _, p in ipairs(points) do
            if Geometry.hpoint_point_intersecting(point, p) then
                return nil
            end
        end
        table.insert(points, point)
    end

    for _, edge in ipairs(edges1) do
        local I = Geometry.hline_segment_triangle_intersection(edge, tri)
        if I ~= nil then
            add_unique(I)
        end
    end
    for _, edge in ipairs(edges2) do
        local I = Geometry.hline_segment_triangle_intersection(edge, self)
        if I ~= nil then
            add_unique(I)
        end
    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
--- @param self Matrix
--- @param line Matrix
--- @return table|nil
function Geometry.hpartition_line_segment_by_line_segment(self, line)
    local I = Geometry.hline_segment_line_segment_intersection(self, line)
    if I ~= nil then
        return Geometry.hpartition_line_segment_by_point(self, I)
    end
    return nil
end

--- Partition a line segment by a triangle
--- @param self Matrix
--- @param tri Matrix
--- @return table|nil
function Geometry.hpartition_line_segment_by_triangle(self, tri)
    local I = Geometry.hline_segment_triangle_intersection(self, tri)
    if I ~= nil then
        return Geometry.hpartition_line_segment_by_point(self, I)
    end
    return nil
end

--- Partition a triangle by another triangle
--- @param self Matrix
--- @param tri Matrix
--- @return table|nil  list of sub-triangle Matrices
function Geometry.hpartition_triangle_by_triangle(self, tri)
    local I = Geometry.htriangle_triangle_intersections(self, tri)
    if I == nil then return nil end
    local IA = I:hto_basis()
    local edges1 = {
        Matrix:_new{self[1], self[2]},
        Matrix:_new{self[2], self[3]},
        Matrix:_new{self[3], self[1]}
    }
    local bases1 = {}
    for i, edge in ipairs(edges1) do
        bases1[i] = edge:hto_basis()
    end

    local points = {}
    local function add_point(point)
        for _, p in ipairs(points) do
            if Geometry.hpoint_point_intersecting(point, 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 = Geometry.hline_line_intersection(IA, edge_basis)
        if int ~= nil and Geometry.hline_segment_point_intersecting(edges1[i], int, false) then
            add_point(int)
        else
            non_intersecting = i
        end
    end

    if #points ~= 2 then
        return nil
    end

    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
        return nil
    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

--- Partition a triangle by vertical edge-planes of another triangle.
--- @param self Matrix
--- @param tri Matrix
--- @return table|nil
function Geometry.hpartition_triangle_by_edge_planes(self, tri)
    local z_min = math.min(self[1][3], self[2][3], self[3][3])
    local z_max = math.max(self[1][3], self[2][3], self[3][3])
    local z_margin = math.max((z_max - z_min) * 0.1, 1e-6)
    z_min = z_min - z_margin
    z_max = z_max + z_margin

    local cutting_tris = {}
    local edges = {
        {tri[1], tri[2]},
        {tri[2], tri[3]},
        {tri[3], tri[1]}
    }

    for _, edge in ipairs(edges) do
        local A = edge[1]
        local B = edge[2]
        if math.abs(A[1] - B[1]) < 1e-12 and math.abs(A[2] - B[2]) < 1e-12 then
            goto continue_edge
        end
        local BL = {A[1], A[2], z_min, 1}
        local BR = {B[1], B[2], z_min, 1}
        local TR = {B[1], B[2], z_max, 1}
        local TL = {A[1], A[2], z_max, 1}
        table.insert(cutting_tris, Matrix:_new{BL, BR, TR})
        table.insert(cutting_tris, Matrix:_new{BL, TR, TL})
        ::continue_edge::
    end

    local pieces = {self:deep_copy()}
    for _, cutter in ipairs(cutting_tris) do
        local new_pieces = {}
        for _, piece in ipairs(pieces) do
            local parts = Geometry.hpartition_triangle_by_triangle(piece, cutter)
            if parts ~= nil then
                for _, part in ipairs(parts) do
                    table.insert(new_pieces, part)
                end
            else
                table.insert(new_pieces, piece)
            end
        end
        pieces = new_pieces
    end

    if #pieces > 1 then
        return pieces
    end
    return nil
end

-- ================================================================
-- Occlusion sorting between simplices
-- ================================================================

--- line-segment vs line-segment occlusion
function Geometry.hline_segment_line_segment_occlusion_sort(self, L)
    local L1A = Vector:_new{self[1][1], self[1][2], 0, 1}
    local L1B = Vector:_new{self[2][1], self[2][2], 0, 1}
    local L2A = Vector:_new{L[1][1], L[1][2], 0, 1}
    local L2B = Vector:_new{L[2][1], L[2][2], 0, 1}
    local L1_dir = L1B:hsub(L1A)
    local L2_dir = L2B:hsub(L2A)

    local RL1A = Vector:_new(self[1])
    local RL1B = Vector:_new(self[2])
    local RL2A = Vector:_new(L[1])
    local RL2B = Vector:_new(L[2])

    local eps = 1e-12
    if not Geometry.hcollinear(L1_dir, L2_dir) then
        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 Geometry.hpoint_point_occlusion_sort(RL1I, RL2I)
        end
    else
        local M1 = Geometry.hpoint_line_segment_occlusion_sort(RL1A, L)
        if M1 ~= nil then return M1 end
        local M2 = Geometry.hpoint_line_segment_occlusion_sort(RL1B, L)
        if M2 ~= nil then return M2 end
        local M3 = Geometry.hpoint_line_segment_occlusion_sort(RL2A, self)
        if M3 ~= nil then return not M3 end
        local M4 = Geometry.hpoint_line_segment_occlusion_sort(RL2B, self)
        if M4 ~= nil then return not M4 end
    end
    return nil
end

--- line-segment vs triangle occlusion
function Geometry.hline_segment_triangle_occlusion_sort(self, T)
    local points1 = { Vector:_new(self[1]), Vector:_new(self[2]) }
    for _, p1 in ipairs(points1) do
        local A = Geometry.hpoint_triangle_occlusion_sort(p1, 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 = Geometry.hline_segment_line_segment_occlusion_sort(self, e2)
        if A ~= nil then return A end
    end
    return nil
end

--- triangle vs triangle occlusion
function Geometry.htriangle_triangle_occlusion_sort(self, 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 = Geometry.hline_segment_triangle_occlusion_sort(e1, 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 = Geometry.hpoint_triangle_occlusion_sort(p1, 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 = Geometry.hpoint_triangle_occlusion_sort(p2, self)
        if A ~= nil then return not A end
    end
    return nil
end

--- Top-level occlusion sort dispatcher
--- @param S1 table simplex record
--- @param S2 table simplex record
--- @return boolean|nil
function Geometry.occlusion_sort_simplices(S1, S2)
    if S1.type == "point" and S2.type == "point" then
        return Geometry.hpoint_point_occlusion_sort(S1.simplex, S2.simplex)
    elseif S1.type == "point" and S2.type == "line segment" then
        return Geometry.hpoint_line_segment_occlusion_sort(S1.simplex, S2.simplex)
    elseif S1.type == "line segment" and S2.type == "point" then
        local A = Geometry.hpoint_line_segment_occlusion_sort(S2.simplex, S1.simplex)
        if A ~= nil then return not A end
    elseif S1.type == "point" and S2.type == "triangle" then
        return Geometry.hpoint_triangle_occlusion_sort(S1.simplex, S2.simplex)
    elseif S1.type == "triangle" and S2.type == "point" then
        local A = Geometry.hpoint_triangle_occlusion_sort(S2.simplex, S1.simplex)
        if A ~= nil then return not A end
    elseif S1.type == "line segment" and S2.type == "line segment" then
        return Geometry.hline_segment_line_segment_occlusion_sort(S1.simplex, S2.simplex)
    elseif S1.type == "line segment" and S2.type == "triangle" then
        local A = Geometry.hline_segment_triangle_occlusion_sort(S1.simplex, S2.simplex)
        if A == nil then return nil else return A end
    elseif S1.type == "triangle" and S2.type == "line segment" then
        local A = Geometry.hline_segment_triangle_occlusion_sort(S2.simplex, S1.simplex)
        if A == nil then return nil else return not A end
    elseif S1.type == "triangle" and S2.type == "triangle" then
        return Geometry.htriangle_triangle_occlusion_sort(S1.simplex, S2.simplex)
    end
end

-- ================================================================
-- 2D grid spatial index for bbox overlap queries
-- ================================================================

--- Build a uniform 2D grid index from a list of simplex records.
--- Each record must have a .bbox2 field (pre-computed).
--- @param simplices table list of simplex records
--- @param cell_size number grid cell width/height
--- @return table grid  { cells = {}, cell_size = N, ... }
function Geometry.build_grid(simplices, cell_size)
    local cells = {}
    local function key(cx, cy)
        return cx .. "," .. cy
    end
    for i, s in ipairs(simplices) do
        local bb = s.bbox2
        if bb then
            local cx0 = math.floor(bb.min[1] / cell_size)
            local cy0 = math.floor(bb.min[2] / cell_size)
            local cx1 = math.floor(bb.max[1] / cell_size)
            local cy1 = math.floor(bb.max[2] / cell_size)
            for cx = cx0, cx1 do
                for cy = cy0, cy1 do
                    local k = key(cx, cy)
                    if not cells[k] then cells[k] = {} end
                    table.insert(cells[k], i)
                end
            end
        end
    end
    return { cells = cells, cell_size = cell_size, key = key }
end

--- Iterate over candidate pairs that share at least one grid cell.
--- Returns a set of {i,j} pairs (i < j) as keys in a table.
--- @param grid table
--- @return table  set of "i,j" keys
function Geometry.grid_candidate_pairs(grid)
    local pairs_seen = {}
    for _, bucket in pairs(grid.cells) do
        for a = 1, #bucket do
            for b = a + 1, #bucket do
                local i, j = bucket[a], bucket[b]
                if i > j then i, j = j, i end
                local k = i .. "," .. j
                pairs_seen[k] = { i, j }
            end
        end
    end
    return pairs_seen
end

-- ================================================================
-- SCC-based occlusion sort with spatial indexing
-- ================================================================

--- Strongly connected components sort for simplices based on occlusion.
--- Uses a 2D grid to reduce pair comparisons, Tarjan's SCC to detect cycles,
--- and edge-plane partitioning to break them.
--- @param simplices table list of simplex records (each must have .bbox2)
--- @param depth number|nil
--- @return table sorted list
function Geometry.scc(simplices, depth)
    depth = depth or 0
    local max_depth = 8
    local n = #simplices

    -- Estimate cell size from average bbox diagonal
    local total_diag = 0
    local diag_count = 0
    for _, s in ipairs(simplices) do
        if s.bbox2 then
            local dx = s.bbox2.max[1] - s.bbox2.min[1]
            local dy = s.bbox2.max[2] - s.bbox2.min[2]
            total_diag = total_diag + math.sqrt(dx*dx + dy*dy)
            diag_count = diag_count + 1
        end
    end
    local cell_size = (diag_count > 0) and (total_diag / diag_count * 2) or 1.0
    if cell_size < 1e-6 then cell_size = 1.0 end

    local grid = Geometry.build_grid(simplices, cell_size)
    local candidate_pairs = Geometry.grid_candidate_pairs(grid)

    local adj = {}
    for i = 1, n do adj[i] = {} end

    for _, pair in pairs(candidate_pairs) do
        local i, j = pair[1], pair[2]
        local si, sj = simplices[i], simplices[j]
        if si.type ~= "label" and sj.type ~= "label"
           and si.type ~= "point" and sj.type ~= "point"
        then
            -- Use cached bboxes for the overlap check
            if si.simplex:bboxes_overlap2(sj.simplex, si.bbox2, sj.bbox2) then
                local cmp = Geometry.occlusion_sort_simplices(si, sj)
                if cmp == true then
                    table.insert(adj[i], j)
                elseif cmp == false then
                    table.insert(adj[j], i)
                end
            end
        end
    end

    -- Tarjan's SCC
    local index_counter = 0
    local stack = {}
    local on_stack = {}
    local indices = {}
    local lowlinks = {}
    local components = {}

    local function strongconnect(v)
        indices[v] = index_counter
        lowlinks[v] = index_counter
        index_counter = index_counter + 1
        table.insert(stack, v)
        on_stack[v] = true

        for _, w in ipairs(adj[v]) do
            if indices[w] == nil then
                strongconnect(w)
                lowlinks[v] = math.min(lowlinks[v], lowlinks[w])
            elseif on_stack[w] then
                lowlinks[v] = math.min(lowlinks[v], indices[w])
            end
        end

        if lowlinks[v] == indices[v] then
            local component = {}
            while true do
                local w = table.remove(stack)
                on_stack[w] = false
                table.insert(component, w)
                if w == v then break end
            end
            table.insert(components, component)
        end
    end

    for i = 1, n do
        if indices[i] == nil then strongconnect(i) end
    end

    -- Cycle resolution
    local has_cycle = false
    for _, comp in ipairs(components) do
        if #comp > 1 then has_cycle = true; break end
    end

    if has_cycle and depth < max_depth then
        local to_remove = {}
        local to_add = {}

        for _, comp in ipairs(components) do
            if #comp > 1 then
                local split_done = false
                for ci = 1, #comp do
                    if split_done then break end
                    local idx = comp[ci]
                    if simplices[idx].type == "triangle" and not to_remove[idx] then
                        for cj = 1, #comp do
                            if cj ~= ci then
                                local other_idx = comp[cj]
                                if simplices[other_idx].type == "triangle" then
                                    local parts = Geometry.hpartition_triangle_by_edge_planes(
                                        simplices[idx].simplex,
                                        simplices[other_idx].simplex)
                                    if parts ~= nil then
                                        to_remove[idx] = true
                                        local meta = {}
                                        for k, v in pairs(simplices[idx]) do
                                            if k ~= "simplex" and k ~= "type" and k ~= "bbox2" then
                                                meta[k] = v
                                            end
                                        end
                                        for _, part in ipairs(parts) do
                                            local ns = {
                                                simplex = part,
                                                type = "triangle",
                                                bbox2 = part:get_bbox2()
                                            }
                                            for k, v in pairs(meta) do ns[k] = v end
                                            table.insert(to_add, ns)
                                        end
                                        split_done = true
                                        break
                                    end
                                end
                            end
                        end
                    end
                end
            end
        end

        if next(to_remove) then
            local new_simplices = {}
            for i, s in ipairs(simplices) do
                if not to_remove[i] then
                    table.insert(new_simplices, s)
                end
            end
            for _, s in ipairs(to_add) do
                table.insert(new_simplices, s)
            end
            return Geometry.scc(new_simplices, depth + 1)
        end
    end

    -- Topological sort (DFS)
    local visited = {}
    local sorted = {}

    local function visit(u)
        if visited[u] then return end
        visited[u] = true
        for _, v in ipairs(adj[u]) do
            visit(v)
        end
        table.insert(sorted, 1, simplices[u])
    end

    for i = 1, n do visit(i) end

    return sorted
end

-- ================================================================
-- Partition simplices by parents (with screen-space overlap check)
-- ================================================================

--- Partition simplices by their parents recursively, retaining all terminal pieces.
--- Only tests pairs that overlap in screen-space (bbox2).
--- Skips self-partitioning.
--- @param simplices table
--- @param parents table
--- @return table
function Geometry.partition_simplices_by_parents(simplices, parents)
    -- Pre-compute parent bboxes
    local parent_bboxes = {}
    for pi, parent in ipairs(parents) do
        if parent.type ~= "point" and parent.type ~= "label" then
            if parent.bbox2 then
                parent_bboxes[pi] = parent.bbox2
            else
                if getmetatable(parent.simplex) == Matrix then
                    parent_bboxes[pi] = parent.simplex:get_bbox2()
                end
            end
        end
    end

    local result = {}

    local function partition_recursive(piece, piece_bbox, start_parent_index)
        local meta = {}
        for k, v in pairs(piece) do
            if k ~= "simplex" and k ~= "type" and k ~= "bbox2" then
                meta[k] = v
            end
        end

        local split_occurred = false
        start_parent_index = start_parent_index or 1

        for pi = start_parent_index, #parents do
            local parent = parents[pi]
            -- Skip self (same simplex table reference)
            if parent.simplex == piece.simplex then goto continue_parent end
            if parent.type == "point" or parent.type == "label" then goto continue_parent end
            if piece.type == "point" or piece.type == "label" then goto continue_parent end

            local pb = parent_bboxes[pi]
            if not pb then goto continue_parent end

            -- Screen-space overlap check
            if not piece.simplex:bboxes_overlap2(parent.simplex, piece_bbox, pb) then
                goto continue_parent
            end

            -- 3D overlap check
            if not piece.simplex:bboxes_overlap3(parent.simplex) then
                goto continue_parent
            end

            local parts = nil
            local piece_type = piece.type
            local parent_type = parent.type

            if parent_type == "point" and piece_type == "line segment" then
                parts = Geometry.hpartition_line_segment_by_point(piece.simplex, parent.simplex)
            elseif parent_type == "line segment" and piece_type == "line segment" then
                parts = Geometry.hpartition_line_segment_by_line_segment(piece.simplex, parent.simplex)
            elseif parent_type == "triangle" and piece_type == "line segment" then
                parts = Geometry.hpartition_line_segment_by_triangle(piece.simplex, parent.simplex)
            elseif parent_type == "triangle" and piece_type == "triangle" then
                parts = Geometry.hpartition_triangle_by_triangle(piece.simplex, parent.simplex)
            end

            if parts ~= nil then
                for _, part in ipairs(parts) do
                    local new_piece = { simplex = part, type = piece_type, bbox2 = part:get_bbox2() }
                    for k, v in pairs(meta) do new_piece[k] = v end
                    partition_recursive(new_piece, new_piece.bbox2, pi + 1)
                end
                split_occurred = true
                break
            end

            ::continue_parent::
        end

        if not split_occurred then
            table.insert(result, piece)
        end
    end

    for _, simplex in ipairs(simplices) do
        local bb = simplex.bbox2
        if not bb and getmetatable(simplex.simplex) == Matrix then
            bb = simplex.simplex:get_bbox2()
        end
        partition_recursive(simplex, bb, 1)
    end

    return result
end

-- ================================================================
-- Install methods on Vector/Matrix metatables for backward compatibility
-- These delegate to the Geometry module functions.
-- ================================================================

function Geometry.install_methods()
    -- Vector methods
    Vector.hpoint_point_intersecting = Geometry.hpoint_point_intersecting
    Vector.hpoint_in_triangular_prism = Geometry.hpoint_in_triangular_prism
    Vector.hpoint_triangle_intersecting = Geometry.hpoint_triangle_intersecting
    Vector.hpoint_point_occlusion_sort = Geometry.hpoint_point_occlusion_sort
    Vector.hcollinear = Geometry.hcollinear
    Vector.hoppositely_directed = Geometry.hoppositely_directed
    Vector.hpoint_line_segment_occlusion_sort = Geometry.hpoint_line_segment_occlusion_sort
    Vector.hpoint_triangle_occlusion_sort = Geometry.hpoint_triangle_occlusion_sort

    -- Matrix methods
    function Matrix:hline_segment_point_intersecting(point)
        return Geometry.hline_segment_point_intersecting(self, point, false)
    end
    function Matrix:hline_segment_point_intersecting2(point)
        return Geometry.hline_segment_point_intersecting(self, point, true)
    end
    Matrix.hpartition_line_segment_by_point = Geometry.hpartition_line_segment_by_point
    Matrix.hline_line_intersection = Geometry.hline_line_intersection
    Matrix.hline_segment_line_segment_intersection = Geometry.hline_segment_line_segment_intersection
    Matrix.hline_plane_intersection = Geometry.hline_plane_intersection
    Matrix.hline_segment_triangle_intersection = Geometry.hline_segment_triangle_intersection
    Matrix.htriangle_triangle_intersections = Geometry.htriangle_triangle_intersections
    Matrix.hpartition_line_segment_by_line_segment = Geometry.hpartition_line_segment_by_line_segment
    Matrix.hpartition_line_segment_by_triangle = Geometry.hpartition_line_segment_by_triangle
    Matrix.hpartition_triangle_by_triangle = Geometry.hpartition_triangle_by_triangle
    Matrix.hpartition_triangle_by_edge_planes = Geometry.hpartition_triangle_by_edge_planes
    Matrix.hline_segment_line_segment_occlusion_sort = Geometry.hline_segment_line_segment_occlusion_sort
    Matrix.hline_segment_triangle_occlusion_sort = Geometry.hline_segment_triangle_occlusion_sort
    Matrix.htriangle_triangle_occlusion_sort = Geometry.htriangle_triangle_occlusion_sort
end

return Geometry