#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>

#ifdef _WIN32
#define EXPORT __declspec(dllexport)
#else
#define EXPORT __attribute__((visibility("default")))
#endif

EXPORT int find_sharpest_seam_c(const double* pts, int M, int stride, double scale) {
    if (M < 3) return 0;

    // Find Y bounding range
    double min_y = pts[1];
    double max_y = pts[1];
    for (int i = 1; i < M; ++i) {
        double y = pts[i * stride + 1];
        if (y < min_y) min_y = y;
        if (y > max_y) max_y = y;
    }
    double y_range = max_y - min_y;
    double corner_bonus = (y_range > 0.0) ? (0.1 * y_range < 2.0 ? 0.1 * y_range : 2.0) : 2.0;

    int best_idx = 0;
    double best_score = -1e30;
    double best_x = 1e30;

    for (int i = 0; i < M; ++i) {
        int prev = (i == 0) ? M - 1 : i - 1;
        int next = (i == M - 1) ? 0 : i + 1;

        double xi = pts[i * stride + 0];
        double yi = pts[i * stride + 1];

        double vin_x = xi - pts[prev * stride + 0];
        double vin_y = yi - pts[prev * stride + 1];
        double vout_x = pts[next * stride + 0] - xi;
        double vout_y = pts[next * stride + 1] - yi;

        double l_in = sqrt(vin_x * vin_x + vin_y * vin_y);
        double l_out = sqrt(vout_x * vout_x + vout_y * vout_y);

        double sharpness = 0.0;
        if (l_in > 1e-7 && l_out > 1e-7) {
            double dot = vin_x * vout_x + vin_y * vout_y;
            double cos_ang = dot / (l_in * l_out);
            if (cos_ang > 1.0) cos_ang = 1.0;
            if (cos_ang < -1.0) cos_ang = -1.0;
            sharpness = 1.0 - cos_ang; // 0 for straight line, 1 for 90 deg corner, 2 for 180 deg turnaround
        }

        double score = yi + sharpness * corner_bonus;

        // Highest score wins (back of the object). Tie-break: smallest X for deterministic consistency across layers
        if (score > best_score + 1e-6) {
            best_score = score;
            best_x = xi;
            best_idx = i;
        } else if (fabs(score - best_score) <= 1e-6) {
            if (xi < best_x) {
                best_x = xi;
                best_idx = i;
            }
        }
    }

    return best_idx;
}

EXPORT void align_path_seam_sharpest_c(const double* in_pts, int M, int stride, double* out_pts, double scale) {
    int best_idx = find_sharpest_seam_c(in_pts, M, stride, scale);
    if (best_idx == 0) {
        memcpy(out_pts, in_pts, (M + 1) * stride * sizeof(double));
        return;
    }
    int split = M - best_idx;
    memcpy(out_pts, in_pts + (best_idx * stride), split * stride * sizeof(double));
    memcpy(out_pts + (split * stride), in_pts, (best_idx + 1) * stride * sizeof(double));
}

// Fast 64-bit hash for 3 float32 values (12 bytes)
static inline uint64_t hash_v3(const float* v) {
    const uint32_t* u = (const uint32_t*)v;
    uint64_t h = (uint64_t)u[0] * 0x9e3779b97f4a7c15ULL;
    h ^= (uint64_t)u[1] * 0xbf58476d1ce4e5b9ULL;
    h = (h ^ (h >> 30)) * 0x94d049bb133111ebULL;
    h ^= (uint64_t)u[2] * 0x4bfb2f35a09a447fULL;
    h = (h ^ (h >> 27)) * 0x94d049bb133111ebULL;
    return h ^ (h >> 31);
}

static inline int v3_eq(const float* a, const float* b) {
    const uint32_t* ua = (const uint32_t*)a;
    const uint32_t* ub = (const uint32_t*)b;
    return (ua[0] == ub[0]) && (ua[1] == ub[1]) && (ua[2] == ub[2]);
}

EXPORT int weld_vertices_c(
    const float* in_verts,
    int num_in_verts,
    float* out_unique_verts,
    uint32_t* out_inv_indices
) {
    if (num_in_verts <= 0) return 0;

    uint32_t table_size = 1024;
    while (table_size < (uint32_t)num_in_verts * 2) {
        table_size <<= 1;
    }
    uint32_t mask = table_size - 1;

    uint32_t* table = (uint32_t*)calloc(table_size, sizeof(uint32_t));
    if (!table) return -1;

    int unique_count = 0;

    for (int i = 0; i < num_in_verts; ++i) {
        const float* v = in_verts + (i * 3);
        uint64_t h = hash_v3(v);
        uint32_t slot = (uint32_t)(h & mask);

        while (1) {
            uint32_t entry = table[slot];
            if (entry == 0) {
                int uid = unique_count++;
                out_unique_verts[uid * 3 + 0] = v[0];
                out_unique_verts[uid * 3 + 1] = v[1];
                out_unique_verts[uid * 3 + 2] = v[2];
                table[slot] = (uint32_t)(uid + 1);
                out_inv_indices[i] = (uint32_t)uid;
                break;
            }
            uint32_t uid = entry - 1;
            const float* existing_v = out_unique_verts + (uid * 3);
            if (v3_eq(v, existing_v)) {
                out_inv_indices[i] = (uint32_t)uid;
                break;
            }
            slot = (slot + 1) & mask;
        }
    }

    free(table);
    return unique_count;
}

EXPORT double polygon_signed_area_2d_c(const double* pts, int N, int stride) {
    if (N < 3) return 0.0;
    double area = pts[(N - 1) * stride + 0] * pts[1] - pts[0] * pts[(N - 1) * stride + 1];
    for (int i = 0; i < N - 1; ++i) {
        area += pts[i * stride + 0] * pts[(i + 1) * stride + 1] - pts[(i + 1) * stride + 0] * pts[i * stride + 1];
    }
    return area * 0.5;
}

static inline int intersect_segments_2d(
    double ax, double ay, double bx, double by,
    double cx, double cy, double dx, double dy,
    double* out_t, double* out_u
) {
    double denom = (bx - ax) * (dy - cy) - (by - ay) * (dx - cx);
    if (fabs(denom) < 1e-12) return 0;
    double t = ((cx - ax) * (dy - cy) - (cy - ay) * (dx - cx)) / denom;
    double u = ((cx - ax) * (by - ay) - (cy - ay) * (bx - ax)) / denom;
    if (t >= -1e-6 && t <= 1.0 + 1e-6 && u >= -1e-6 && u <= 1.0 + 1e-6) {
        if (t < 0.0) t = 0.0;
        if (t > 1.0) t = 1.0;
        if (u < 0.0) u = 0.0;
        if (u > 1.0) u = 1.0;
        *out_t = t;
        *out_u = u;
        return 1;
    }
    return 0;
}

static inline int intersect_segment_triangle(
    const double* p0, const double* p1,
    const double* v0, const double* v1, const double* v2,
    double* out_t, double* out_pt
) {
    double e1x = v1[0] - v0[0], e1y = v1[1] - v0[1], e1z = v1[2] - v0[2];
    double e2x = v2[0] - v0[0], e2y = v2[1] - v0[1], e2z = v2[2] - v0[2];
    double nx = e1y * e2z - e1z * e2y;
    double ny = e1z * e2x - e1x * e2z;
    double nz = e1x * e2y - e1y * e2x;
    double n_len_sq = nx * nx + ny * ny + nz * nz;
    if (n_len_sq < 1e-14) return 0;

    double d0 = (p0[0] - v0[0]) * nx + (p0[1] - v0[1]) * ny + (p0[2] - v0[2]) * nz;
    double d1 = (p1[0] - v0[0]) * nx + (p1[1] - v0[1]) * ny + (p1[2] - v0[2]) * nz;

    if ((d0 > 1e-6 && d1 > 1e-6) || (d0 < -1e-6 && d1 < -1e-6)) return 0;

    double denom = d0 - d1;
    if (fabs(denom) > 1e-12) {
        double t = d0 / denom;
        if (t < -1e-6 || t > 1.0 + 1e-6) return 0;
        if (t < 0.0) t = 0.0;
        if (t > 1.0) t = 1.0;

        double ix = p0[0] + t * (p1[0] - p0[0]);
        double iy = p0[1] + t * (p1[1] - p0[1]);
        double iz = p0[2] + t * (p1[2] - p0[2]);

        double c0x = (v1[1] - v0[1]) * (iz - v0[2]) - (v1[2] - v0[2]) * (iy - v0[1]);
        double c0y = (v1[2] - v0[2]) * (ix - v0[0]) - (v1[0] - v0[0]) * (iz - v0[2]);
        double c0z = (v1[0] - v0[0]) * (iy - v0[1]) - (v1[1] - v0[1]) * (ix - v0[0]);
        double dot0 = c0x * nx + c0y * ny + c0z * nz;

        double c1x = (v2[1] - v1[1]) * (iz - v1[2]) - (v2[2] - v1[2]) * (iy - v1[1]);
        double c1y = (v2[2] - v1[2]) * (ix - v1[0]) - (v2[0] - v1[0]) * (iz - v1[2]);
        double c1z = (v2[0] - v1[0]) * (iy - v1[1]) - (v2[1] - v1[1]) * (ix - v1[0]);
        double dot1 = c1x * nx + c1y * ny + c1z * nz;

        double c2x = (v0[1] - v2[1]) * (iz - v2[2]) - (v0[2] - v2[2]) * (iy - v2[1]);
        double c2y = (v0[2] - v2[2]) * (ix - v2[0]) - (v0[0] - v2[0]) * (iz - v2[2]);
        double c2z = (v0[0] - v2[0]) * (iy - v2[1]) - (v0[1] - v2[1]) * (ix - v2[0]);
        double dot2 = c2x * nx + c2y * ny + c2z * nz;

        double tol = -1e-6 * n_len_sq;
        if (dot0 >= tol && dot1 >= tol && dot2 >= tol) {
            *out_t = t;
            out_pt[0] = ix;
            out_pt[1] = iy;
            out_pt[2] = iz;
            return 1;
        }
    } else if (fabs(d0) < 1e-6) {
        // Coplanar segment - test 3 edges in 2D projection along dominant normal axis
        int ax0 = 0, ax1 = 1;
        if (fabs(nx) >= fabs(ny) && fabs(nx) >= fabs(nz)) {
            ax0 = 1; ax1 = 2;
        } else if (fabs(ny) >= fabs(nx) && fabs(ny) >= fabs(nz)) {
            ax0 = 0; ax1 = 2;
        }

        double best_t = 1e30;
        int found = 0;
        double t_cand, u_cand;

        const double* verts[3] = {v0, v1, v2};
        for (int e = 0; e < 3; ++e) {
            const double* va = verts[e];
            const double* vb = verts[(e + 1) % 3];
            if (intersect_segments_2d(p0[ax0], p0[ax1], p1[ax0], p1[ax1],
                                      va[ax0], va[ax1], vb[ax0], vb[ax1],
                                      &t_cand, &u_cand)) {
                if (t_cand < best_t) {
                    best_t = t_cand;
                    found = 1;
                }
            }
        }
        if (found) {
            *out_t = best_t;
            out_pt[0] = p0[0] + best_t * (p1[0] - p0[0]);
            out_pt[1] = p0[1] + best_t * (p1[1] - p0[1]);
            out_pt[2] = p0[2] + best_t * (p1[2] - p0[2]);
            return 1;
        }
    }
    return 0;
}

EXPORT int slice_triangles_at_z_c(
    const double* tri_verts,
    int num_tris,
    double pz,
    double* out_segs
) {
    int num_segs = 0;
    for (int j = 0; j < num_tris; ++j) {
        const double* v[3] = {
            tri_verts + (j * 9 + 0),
            tri_verts + (j * 9 + 3),
            tri_verts + (j * 9 + 6)
        };

        double tz_min = v[0][2] < v[1][2] ? (v[0][2] < v[2][2] ? v[0][2] : v[2][2]) : (v[1][2] < v[2][2] ? v[1][2] : v[2][2]);
        double tz_max = v[0][2] > v[1][2] ? (v[0][2] > v[2][2] ? v[0][2] : v[2][2]) : (v[1][2] > v[2][2] ? v[1][2] : v[2][2]);
        if (tz_max < pz - 1e-4 || tz_min > pz + 1e-4) continue;

        double pts2d[3][2];
        int n_pts2d = 0;

        for (int e = 0; e < 3; ++e) {
            const double* va = v[e];
            const double* vb = v[(e + 1) % 3];
            double dza = va[2] - pz;
            double dzb = vb[2] - pz;

            if ((dza <= 0.0 && dzb >= 0.0) || (dza >= 0.0 && dzb <= 0.0)) {
                double denom = vb[2] - va[2];
                double u = (fabs(denom) > 1e-12) ? (pz - va[2]) / denom : 0.0;
                if (u < 0.0) u = 0.0;
                if (u > 1.0) u = 1.0;
                double qx = va[0] + u * (vb[0] - va[0]);
                double qy = va[1] + u * (vb[1] - va[1]);

                if (n_pts2d == 0 || (fabs(qx - pts2d[0][0]) > 1e-6 || fabs(qy - pts2d[0][1]) > 1e-6)) {
                    if (n_pts2d < 2) {
                        pts2d[n_pts2d][0] = qx;
                        pts2d[n_pts2d][1] = qy;
                        n_pts2d++;
                    }
                }
            }
        }

        if (n_pts2d == 2) {
            out_segs[num_segs * 4 + 0] = pts2d[0][0];
            out_segs[num_segs * 4 + 1] = pts2d[0][1];
            out_segs[num_segs * 4 + 2] = pts2d[1][0];
            out_segs[num_segs * 4 + 3] = pts2d[1][1];
            num_segs++;
        }
    }
    return num_segs;
}

EXPORT int intersect_path_with_2d_segments_c(
    const double* path_pts,
    int num_pts,
    int stride,
    const double* mesh_segs,
    int num_segs,
    double pz,
    int mode,
    const double* ref_point,
    int* out_seg_idx,
    double* out_t,
    double* out_pt
) {
    if (num_pts < 4 || num_segs <= 0) return 0;
    int M = num_pts - 1;

    double p_min_x = path_pts[0], p_max_x = path_pts[0];
    double p_min_y = path_pts[1], p_max_y = path_pts[1];
    for (int i = 1; i < num_pts; ++i) {
        double px = path_pts[i * stride + 0];
        double py = path_pts[i * stride + 1];
        if (px < p_min_x) p_min_x = px;
        if (px > p_max_x) p_max_x = px;
        if (py < p_min_y) p_min_y = py;
        if (py > p_max_y) p_max_y = py;
    }
    p_min_x -= 1e-4; p_max_x += 1e-4;
    p_min_y -= 1e-4; p_max_y += 1e-4;

    int found = 0;
    int best_seg = -1;
    double best_t = 0.0;
    double best_pt[3] = {0.0, 0.0, 0.0};
    double best_dist_sq = 1e30;

    double rx = ref_point ? ref_point[0] : path_pts[0];
    double ry = ref_point ? ref_point[1] : path_pts[1];
    double rz = ref_point ? ref_point[2] : pz;

    for (int i = 0; i < M; ++i) {
        const double* p0 = path_pts + (i * stride);
        const double* p1 = path_pts + ((i + 1) * stride);

        double s_min_x = (p0[0] < p1[0] ? p0[0] : p1[0]) - 1e-4;
        double s_max_x = (p0[0] > p1[0] ? p0[0] : p1[0]) + 1e-4;
        double s_min_y = (p0[1] < p1[1] ? p0[1] : p1[1]) - 1e-4;
        double s_max_y = (p0[1] > p1[1] ? p0[1] : p1[1]) + 1e-4;

        for (int s = 0; s < num_segs; ++s) {
            double q0x = mesh_segs[s * 4 + 0];
            double q0y = mesh_segs[s * 4 + 1];
            double q1x = mesh_segs[s * 4 + 2];
            double q1y = mesh_segs[s * 4 + 3];

            double q_min_x = (q0x < q1x ? q0x : q1x) - 1e-4;
            double q_max_x = (q0x > q1x ? q0x : q1x) + 1e-4;
            if (q_max_x < s_min_x || q_min_x > s_max_x) continue;

            double q_min_y = (q0y < q1y ? q0y : q1y) - 1e-4;
            double q_max_y = (q0y > q1y ? q0y : q1y) + 1e-4;
            if (q_max_y < s_min_y || q_min_y > s_max_y) continue;

            double t_cand, u_cand;
            if (intersect_segments_2d(p0[0], p0[1], p1[0], p1[1], q0x, q0y, q1x, q1y, &t_cand, &u_cand)) {
                double ix = p0[0] + t_cand * (p1[0] - p0[0]);
                double iy = p0[1] + t_cand * (p1[1] - p0[1]);
                double iz = pz;

                if (mode == 1) { // FIRST
                    *out_seg_idx = i;
                    *out_t = t_cand;
                    out_pt[0] = ix;
                    out_pt[1] = iy;
                    out_pt[2] = iz;
                    return 1;
                }

                double dx = ix - rx;
                double dy = iy - ry;
                double dz = iz - rz;
                double d_sq = dx * dx + dy * dy + dz * dz;

                if (!found || d_sq < best_dist_sq) {
                    found = 1;
                    best_dist_sq = d_sq;
                    best_seg = i;
                    best_t = t_cand;
                    best_pt[0] = ix;
                    best_pt[1] = iy;
                    best_pt[2] = iz;
                }
            }
        }
    }

    if (found) {
        *out_seg_idx = best_seg;
        *out_t = best_t;
        out_pt[0] = best_pt[0];
        out_pt[1] = best_pt[1];
        out_pt[2] = best_pt[2];
        return 1;
    }
    return 0;
}

EXPORT int find_path_mesh_intersection_c(
    const double* path_pts,
    int num_pts,
    int stride,
    const double* tri_verts,
    int num_tris,
    int mode,
    const double* ref_point,
    int* out_seg_idx,
    double* out_t,
    double* out_pt
) {
    if (num_pts < 4 || num_tris <= 0) return 0;
    int M = num_pts - 1;

    // 1. Path bounding box
    double p_min_x = path_pts[0], p_max_x = path_pts[0];
    double p_min_y = path_pts[1], p_max_y = path_pts[1];
    double p_min_z = path_pts[2], p_max_z = path_pts[2];
    for (int i = 1; i < num_pts; ++i) {
        double px = path_pts[i * stride + 0];
        double py = path_pts[i * stride + 1];
        double pz = path_pts[i * stride + 2];
        if (px < p_min_x) p_min_x = px;
        if (px > p_max_x) p_max_x = px;
        if (py < p_min_y) p_min_y = py;
        if (py > p_max_y) p_max_y = py;
        if (pz < p_min_z) p_min_z = pz;
        if (pz > p_max_z) p_max_z = pz;
    }
    double p_eps = 1e-4;
    p_min_x -= p_eps; p_max_x += p_eps;
    p_min_y -= p_eps; p_max_y += p_eps;
    p_min_z -= p_eps; p_max_z += p_eps;

    // 2. Pre-filter triangles that overlap the path bounding box
    int* active_tris = (int*)malloc(num_tris * sizeof(int));
    if (!active_tris) return 0;
    int num_active = 0;

    for (int j = 0; j < num_tris; ++j) {
        const double* v0 = tri_verts + (j * 9 + 0);
        const double* v1 = tri_verts + (j * 9 + 3);
        const double* v2 = tri_verts + (j * 9 + 6);

        double tz_min = v0[2] < v1[2] ? (v0[2] < v2[2] ? v0[2] : v2[2]) : (v1[2] < v2[2] ? v1[2] : v2[2]);
        double tz_max = v0[2] > v1[2] ? (v0[2] > v2[2] ? v0[2] : v2[2]) : (v1[2] > v2[2] ? v1[2] : v2[2]);
        if (tz_max < p_min_z || tz_min > p_max_z) continue;

        double tx_min = v0[0] < v1[0] ? (v0[0] < v2[0] ? v0[0] : v2[0]) : (v1[0] < v2[0] ? v1[0] : v2[0]);
        double tx_max = v0[0] > v1[0] ? (v0[0] > v2[0] ? v0[0] : v2[0]) : (v1[0] > v2[0] ? v1[0] : v2[0]);
        if (tx_max < p_min_x || tx_min > p_max_x) continue;

        double ty_min = v0[1] < v1[1] ? (v0[1] < v2[1] ? v0[1] : v2[1]) : (v1[1] < v2[1] ? v1[1] : v2[1]);
        double ty_max = v0[1] > v1[1] ? (v0[1] > v2[1] ? v0[1] : v2[1]) : (v1[1] > v2[1] ? v1[1] : v2[1]);
        if (ty_max < p_min_y || ty_min > p_max_y) continue;

        active_tris[num_active++] = j;
    }

    if (num_active == 0) {
        free(active_tris);
        return 0;
    }

    int found = 0;
    int best_seg = -1;
    double best_t = 0.0;
    double best_pt[3] = {0.0, 0.0, 0.0};
    double best_dist_sq = 1e30;

    double rx = ref_point ? ref_point[0] : path_pts[0];
    double ry = ref_point ? ref_point[1] : path_pts[1];
    double rz = ref_point ? ref_point[2] : path_pts[2];

    int is_planar = (p_max_z - p_min_z) < 1e-3;

    if (is_planar) {
        double pz = path_pts[2];
        double* mesh_segs = (double*)malloc(num_active * 4 * sizeof(double));
        int num_segs = 0;

        if (mesh_segs) {
            for (int a = 0; a < num_active; ++a) {
                int j = active_tris[a];
                const double* v[3] = {
                    tri_verts + (j * 9 + 0),
                    tri_verts + (j * 9 + 3),
                    tri_verts + (j * 9 + 6)
                };

                double pts2d[3][2];
                int n_pts2d = 0;

                for (int e = 0; e < 3; ++e) {
                    const double* va = v[e];
                    const double* vb = v[(e + 1) % 3];
                    double dza = va[2] - pz;
                    double dzb = vb[2] - pz;

                    if ((dza <= 0.0 && dzb >= 0.0) || (dza >= 0.0 && dzb <= 0.0)) {
                        double denom = vb[2] - va[2];
                        double u = (fabs(denom) > 1e-12) ? (pz - va[2]) / denom : 0.0;
                        if (u < 0.0) u = 0.0;
                        if (u > 1.0) u = 1.0;
                        double qx = va[0] + u * (vb[0] - va[0]);
                        double qy = va[1] + u * (vb[1] - va[1]);

                        if (n_pts2d == 0 || (fabs(qx - pts2d[0][0]) > 1e-6 || fabs(qy - pts2d[0][1]) > 1e-6)) {
                            if (n_pts2d < 2) {
                                pts2d[n_pts2d][0] = qx;
                                pts2d[n_pts2d][1] = qy;
                                n_pts2d++;
                            }
                        }
                    }
                }

                if (n_pts2d == 2) {
                    mesh_segs[num_segs * 4 + 0] = pts2d[0][0];
                    mesh_segs[num_segs * 4 + 1] = pts2d[0][1];
                    mesh_segs[num_segs * 4 + 2] = pts2d[1][0];
                    mesh_segs[num_segs * 4 + 3] = pts2d[1][1];
                    num_segs++;
                }
            }

            for (int i = 0; i < M; ++i) {
                const double* p0 = path_pts + (i * stride);
                const double* p1 = path_pts + ((i + 1) * stride);

                double s_min_x = (p0[0] < p1[0] ? p0[0] : p1[0]) - 1e-4;
                double s_max_x = (p0[0] > p1[0] ? p0[0] : p1[0]) + 1e-4;
                double s_min_y = (p0[1] < p1[1] ? p0[1] : p1[1]) - 1e-4;
                double s_max_y = (p0[1] > p1[1] ? p0[1] : p1[1]) + 1e-4;

                for (int s = 0; s < num_segs; ++s) {
                    double q0x = mesh_segs[s * 4 + 0];
                    double q0y = mesh_segs[s * 4 + 1];
                    double q1x = mesh_segs[s * 4 + 2];
                    double q1y = mesh_segs[s * 4 + 3];

                    double q_min_x = (q0x < q1x ? q0x : q1x) - 1e-4;
                    double q_max_x = (q0x > q1x ? q0x : q1x) + 1e-4;
                    if (q_max_x < s_min_x || q_min_x > s_max_x) continue;

                    double q_min_y = (q0y < q1y ? q0y : q1y) - 1e-4;
                    double q_max_y = (q0y > q1y ? q0y : q1y) + 1e-4;
                    if (q_max_y < s_min_y || q_min_y > s_max_y) continue;

                    double t_cand, u_cand;
                    if (intersect_segments_2d(p0[0], p0[1], p1[0], p1[1], q0x, q0y, q1x, q1y, &t_cand, &u_cand)) {
                        double ix = p0[0] + t_cand * (p1[0] - p0[0]);
                        double iy = p0[1] + t_cand * (p1[1] - p0[1]);
                        double iz = pz;

                        if (mode == 1) { // FIRST
                            *out_seg_idx = i;
                            *out_t = t_cand;
                            out_pt[0] = ix;
                            out_pt[1] = iy;
                            out_pt[2] = iz;
                            free(mesh_segs);
                            free(active_tris);
                            return 1;
                        }

                        double dx = ix - rx;
                        double dy = iy - ry;
                        double dz = iz - rz;
                        double d_sq = dx * dx + dy * dy + dz * dz;

                        if (!found || d_sq < best_dist_sq) {
                            found = 1;
                            best_dist_sq = d_sq;
                            best_seg = i;
                            best_t = t_cand;
                            best_pt[0] = ix;
                            best_pt[1] = iy;
                            best_pt[2] = iz;
                        }
                    }
                }
            }
            free(mesh_segs);
        }
    }

    if (!found) {
        for (int i = 0; i < M; ++i) {
            const double* p0 = path_pts + (i * stride);
            const double* p1 = path_pts + ((i + 1) * stride);

            double s_min_x = (p0[0] < p1[0] ? p0[0] : p1[0]) - 1e-4;
            double s_max_x = (p0[0] > p1[0] ? p0[0] : p1[0]) + 1e-4;
            double s_min_y = (p0[1] < p1[1] ? p0[1] : p1[1]) - 1e-4;
            double s_max_y = (p0[1] > p1[1] ? p0[1] : p1[1]) + 1e-4;
            double s_min_z = (p0[2] < p1[2] ? p0[2] : p1[2]) - 1e-4;
            double s_max_z = (p0[2] > p1[2] ? p0[2] : p1[2]) + 1e-4;

            for (int a = 0; a < num_active; ++a) {
                int j = active_tris[a];
                const double* v0 = tri_verts + (j * 9 + 0);
                const double* v1 = tri_verts + (j * 9 + 3);
                const double* v2 = tri_verts + (j * 9 + 6);

                double cand_t = 0.0;
                double cand_pt[3] = {0.0, 0.0, 0.0};
                if (intersect_segment_triangle(p0, p1, v0, v1, v2, &cand_t, cand_pt)) {
                    if (mode == 1) { // FIRST
                        *out_seg_idx = i;
                        *out_t = cand_t;
                        out_pt[0] = cand_pt[0];
                        out_pt[1] = cand_pt[1];
                        out_pt[2] = cand_pt[2];
                        free(active_tris);
                        return 1;
                    }

                    double dx = cand_pt[0] - rx;
                    double dy = cand_pt[1] - ry;
                    double dz = cand_pt[2] - rz;
                    double d_sq = dx * dx + dy * dy + dz * dz;

                    if (!found || d_sq < best_dist_sq) {
                        found = 1;
                        best_dist_sq = d_sq;
                        best_seg = i;
                        best_t = cand_t;
                        best_pt[0] = cand_pt[0];
                        best_pt[1] = cand_pt[1];
                        best_pt[2] = cand_pt[2];
                    }
                }
            }
        }
    }

    free(active_tris);

    if (found) {
        *out_seg_idx = best_seg;
        *out_t = best_t;
        out_pt[0] = best_pt[0];
        out_pt[1] = best_pt[1];
        out_pt[2] = best_pt[2];
        return 1;
    }
    return 0;
}

EXPORT int reposition_path_seam_c(
    const double* in_pts,
    int num_pts,
    int stride,
    int seg_idx,
    double t,
    const double* intersect_pt,
    double* out_pts
) {
    if (num_pts < 4) {
        memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
        return num_pts;
    }
    int M = num_pts - 1;

    // Check snapping to existing vertices
    if (t <= 1e-6) {
        int k = seg_idx;
        if (k == 0) {
            memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
            return num_pts;
        }
        int split = M - k;
        memcpy(out_pts, in_pts + (k * stride), split * stride * sizeof(double));
        memcpy(out_pts + (split * stride), in_pts, (k + 1) * stride * sizeof(double));
        return num_pts;
    }
    if (1.0 - t <= 1e-6) {
        int next_k = (seg_idx + 1) % M;
        if (next_k == 0) {
            memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
            return num_pts;
        }
        int split = M - next_k;
        memcpy(out_pts, in_pts + (next_k * stride), split * stride * sizeof(double));
        memcpy(out_pts + (split * stride), in_pts, (next_k + 1) * stride * sizeof(double));
        return num_pts;
    }

    // Insert new vertex at intersect_pt
    // Output: [intersect_pt, in_pts[k+1..M-1], in_pts[0..k], intersect_pt]
    int k = seg_idx;
    int out_idx = 0;

    // 1. First point: intersect_pt
    for (int s = 0; s < stride; ++s) {
        out_pts[out_idx * stride + s] = (s < 3) ? intersect_pt[s] : in_pts[k * stride + s];
    }
    out_idx++;

    // 2. Points from k+1 to M-1
    int count_rest = M - (k + 1);
    if (count_rest > 0) {
        memcpy(out_pts + (out_idx * stride), in_pts + ((k + 1) * stride), count_rest * stride * sizeof(double));
        out_idx += count_rest;
    }

    // 3. Points from 0 to k
    int count_prefix = k + 1;
    memcpy(out_pts + (out_idx * stride), in_pts, count_prefix * stride * sizeof(double));
    out_idx += count_prefix;

    // 4. Last point: intersect_pt
    for (int s = 0; s < stride; ++s) {
        out_pts[out_idx * stride + s] = (s < 3) ? intersect_pt[s] : in_pts[k * stride + s];
    }
    out_idx++;

    return out_idx;
}

EXPORT int shift_path_seam_by_distance_c(
    const double* in_pts,
    int num_pts,
    int stride,
    double dist_mm,
    double* out_pts
) {
    if (num_pts < 4 || fabs(dist_mm) < 1e-6) {
        memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
        return num_pts;
    }
    int M = num_pts - 1;
    double total_len = 0.0;
    for (int i = 0; i < M; ++i) {
        double dx = in_pts[(i + 1) * stride + 0] - in_pts[i * stride + 0];
        double dy = in_pts[(i + 1) * stride + 1] - in_pts[i * stride + 1];
        double dz = in_pts[(i + 1) * stride + 2] - in_pts[i * stride + 2];
        total_len += sqrt(dx * dx + dy * dy + dz * dz);
    }
    if (total_len <= 1e-6) {
        memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
        return num_pts;
    }

    double target_d = fmod(dist_mm, total_len);
    if (target_d < 0.0) target_d += total_len;

    if (target_d <= 1e-6 || fabs(target_d - total_len) <= 1e-6) {
        memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
        return num_pts;
    }

    double cum = 0.0;
    for (int i = 0; i < M; ++i) {
        double dx = in_pts[(i + 1) * stride + 0] - in_pts[i * stride + 0];
        double dy = in_pts[(i + 1) * stride + 1] - in_pts[i * stride + 1];
        double dz = in_pts[(i + 1) * stride + 2] - in_pts[i * stride + 2];
        double seg_l = sqrt(dx * dx + dy * dy + dz * dz);
        if (cum + seg_l >= target_d || i == M - 1) {
            double t = (seg_l > 1e-7) ? (target_d - cum) / seg_l : 0.0;
            if (t < 0.0) t = 0.0;
            if (t > 1.0) t = 1.0;

            double ipt[3];
            for (int s = 0; s < 3; ++s) {
                ipt[s] = (1.0 - t) * in_pts[i * stride + s] + t * in_pts[(i + 1) * stride + s];
            }
            return reposition_path_seam_c(in_pts, num_pts, stride, i, t, ipt, out_pts);
        }
        cum += seg_l;
    }

    memcpy(out_pts, in_pts, num_pts * stride * sizeof(double));
    return num_pts;
}