import { BinaryTopology } from "../BinaryTopology.js";
import { bt_mesh_compute_face_islands } from "../query/bt_mesh_compute_face_islands.js";
import { bt_query_edge_is_boundary } from "../query/bt_query_edge_is_boundary.js";
import { bt_mesh_from_unindexed_geometry } from "./bt_mesh_from_unindexed_geometry.js";
import { bt_mesh_fuse_duplicate_edges } from "./edge/bt_mesh_fuse_duplicate_edges.js";
import { bt_merge_verts_by_distance } from "./vertex/bt_merge_verts_by_distance.js";

/**
 * Distance under which rebuilt vertices are fused, matching the navmesh build pipeline. Bridge corner
 * coordinates are copied verbatim from existing boundary vertices, so they fuse exactly.
 * @type {number}
 */
const VERTEX_MERGE_DISTANCE = 1e-6;

/**
 * Tolerance added to the step thresholds so exactly-on-the-limit connections (e.g. a perfectly aligned
 * step with zero lateral gap, tested against max_step_distance = 0) are accepted.
 * @type {number}
 */
const THRESHOLD_EPSILON = 1e-6;

/**
 * Decompose the vector (px,py,pz) - (qx,qy,qz) into components along and perpendicular to `up` and,
 * if it is within the step thresholds, return the perpendicular (lateral) distance; otherwise -1.
 */
function connection_lateral_distance(
    px, py, pz,
    qx, qy, qz,
    up_x, up_y, up_z,
    max_step_height, max_step_distance
) {
    const dx = px - qx;
    const dy = py - qy;
    const dz = pz - qz;

    const along_up = dx * up_x + dy * up_y + dz * up_z;
    const vertical = Math.abs(along_up);
    const lateral = Math.sqrt(Math.max(0, (dx * dx + dy * dy + dz * dz) - along_up * along_up));

    if (vertical <= max_step_height + THRESHOLD_EPSILON && lateral <= max_step_distance + THRESHOLD_EPSILON) {
        return lateral;
    }

    return -1;
}

/**
 * Evaluate whether two boundary edges can be bridged and, if so, which endpoint pairing to use.
 * Writes the cheaper valid pairing into `out_best` as `[pairing, cost]` (pairing 1 = a1<->b1 / a2<->b2,
 * pairing 2 = a1<->b2 / a2<->b1) and returns true; returns false (leaving `out_best` untouched) when
 * neither pairing fits the thresholds. `out_best` is supplied by the caller to avoid per-pair allocation.
 *
 * @returns {boolean}
 */
function evaluate_pair(a, b, up_x, up_y, up_z, max_step_height, max_step_distance, out_best) {
    // pairing 1: a.p1 <-> b.p1, a.p2 <-> b.p2
    const c1_1 = connection_lateral_distance(b.x1, b.y1, b.z1, a.x1, a.y1, a.z1, up_x, up_y, up_z, max_step_height, max_step_distance);
    const c1_2 = connection_lateral_distance(b.x2, b.y2, b.z2, a.x2, a.y2, a.z2, up_x, up_y, up_z, max_step_height, max_step_distance);

    // pairing 2: a.p1 <-> b.p2, a.p2 <-> b.p1
    const c2_1 = connection_lateral_distance(b.x2, b.y2, b.z2, a.x1, a.y1, a.z1, up_x, up_y, up_z, max_step_height, max_step_distance);
    const c2_2 = connection_lateral_distance(b.x1, b.y1, b.z1, a.x2, a.y2, a.z2, up_x, up_y, up_z, max_step_height, max_step_distance);

    let found = false;
    let pairing = 0;
    let cost = Infinity;

    if (c1_1 >= 0 && c1_2 >= 0) {
        pairing = 1;
        cost = c1_1 + c1_2;
        found = true;
    }

    if (c2_1 >= 0 && c2_2 >= 0) {
        const cost_2 = c2_1 + c2_2;
        if (!found || cost_2 < cost) {
            pairing = 2;
            cost = cost_2;
            found = true;
        }
    }

    if (found) {
        out_best[0] = pairing;
        out_best[1] = cost;
    }

    return found;
}

/**
 * Append a triangle to the soup, choosing the winding so its normal points along `up` (keeps bridge
 * faces walkable / consistently oriented with the floor islands they connect).
 */
function push_triangle_facing_up(
    soup,
    p0x, p0y, p0z,
    p1x, p1y, p1z,
    p2x, p2y, p2z,
    up_x, up_y, up_z
) {
    const e1x = p1x - p0x, e1y = p1y - p0y, e1z = p1z - p0z;
    const e2x = p2x - p0x, e2y = p2y - p0y, e2z = p2z - p0z;

    const nx = e1y * e2z - e1z * e2y;
    const ny = e1z * e2x - e1x * e2z;
    const nz = e1x * e2y - e1y * e2x;

    if (nx * up_x + ny * up_y + nz * up_z >= 0) {
        soup.push(p0x, p0y, p0z, p1x, p1y, p1z, p2x, p2y, p2z);
    } else {
        // flip winding so the normal faces up (preserves the undirected edges, only reverses direction)
        soup.push(p0x, p0y, p0z, p2x, p2y, p2z, p1x, p1y, p1z);
    }
}

/**
 * Connect islands that are not topologically joined but lie within a small step (vertical) and/or gap
 * (lateral) of one another, so an agent can path across stairs and small breaks in the floor.
 *
 * The two unused params on the build path (`agent_max_step_height`, `agent_max_step_distance`) drive this.
 * Boundary edges of distinct islands whose endpoints can be paired within the thresholds are stitched
 * with a bridge quad (two triangles). Connectivity is realised by rebuilding the mesh from its triangle
 * soup plus the bridge quads and re-running the same vertex-merge / edge-fuse the build already uses -
 * the bridge corners are copied verbatim from existing boundary vertices, so they fuse onto the islands.
 *
 * Mutates `mesh` in place. Triangulated input is assumed (as produced by the navmesh build pipeline).
 *
 * @param {BinaryTopology} mesh
 * @param {{max_step_height:number, max_step_distance:number, up:Vector3}} options
 * @returns {number} number of bridges added
 */
export function bt_mesh_bridge_islands(mesh, { max_step_height, max_step_distance, up }) {
    const islands = bt_mesh_compute_face_islands(mesh);

    if (islands.length < 2) {
        // a single (or empty) connected component - nothing to bridge
        return 0;
    }

    // normalize up so the along/perpendicular split uses world distances
    let up_x = up.x;
    let up_y = up.y;
    let up_z = up.z;

    const up_length = Math.sqrt(up_x * up_x + up_y * up_y + up_z * up_z);
    up_x /= up_length;
    up_y /= up_length;
    up_z /= up_length;

    // face -> island index
    const island_of_face = new Map();
    for (let i = 0; i < islands.length; i++) {
        const faces = islands[i];
        for (let k = 0; k < faces.length; k++) {
            island_of_face.set(faces[k], i);
        }
    }

    // collect boundary edges, tagged with their island and endpoint coordinates
    const boundary = [];
    const edge_count = mesh.edges.size;
    const coord_1 = [0, 0, 0];
    const coord_2 = [0, 0, 0];

    for (let e = 0; e < edge_count; e++) {
        if (!mesh.edges.is_allocated(e)) {
            continue;
        }

        if (!bt_query_edge_is_boundary(mesh, e)) {
            continue;
        }

        const loop = mesh.edge_read_loop(e);
        const face = mesh.loop_read_face(loop);
        const island = island_of_face.get(face);

        mesh.vertex_read_coordinate(coord_1, 0, mesh.edge_read_vertex1(e));
        mesh.vertex_read_coordinate(coord_2, 0, mesh.edge_read_vertex2(e));

        boundary.push({
            island,
            x1: coord_1[0], y1: coord_1[1], z1: coord_1[2],
            x2: coord_2[0], y2: coord_2[1], z2: coord_2[2],
        });
    }

    // for each boundary edge, find its cheapest bridgeable partner in another island
    const bridges = [];
    const bridged_pairs = new Set();

    // reused [pairing, cost] scratch, written by evaluate_pair, so the inner loop allocates nothing
    const pair_result = [0, 0];

    for (let i = 0; i < boundary.length; i++) {
        let best_j = -1;
        let best_cost = Infinity;
        let best_pairing = 0;

        for (let j = 0; j < boundary.length; j++) {
            if (boundary[j].island === boundary[i].island) {
                continue;
            }

            if (evaluate_pair(boundary[i], boundary[j], up_x, up_y, up_z, max_step_height, max_step_distance, pair_result)) {
                const cost = pair_result[1];

                if (cost < best_cost) {
                    best_cost = cost;
                    best_j = j;
                    best_pairing = pair_result[0];
                }
            }
        }

        if (best_j === -1) {
            continue;
        }

        const key = i < best_j ? `${i}_${best_j}` : `${best_j}_${i}`;
        if (bridged_pairs.has(key)) {
            continue;
        }
        bridged_pairs.add(key);

        bridges.push({ a: boundary[i], b: boundary[best_j], pairing: best_pairing });
    }

    if (bridges.length === 0) {
        return 0;
    }

    // extract the current mesh as a triangle soup
    const soup = [];
    const face_count = mesh.faces.size;
    const coord = [0, 0, 0];

    for (let f = 0; f < face_count; f++) {
        if (!mesh.faces.is_allocated(f)) {
            continue;
        }

        const loop_start = mesh.face_read_loop(f);
        let loop = loop_start;

        do {
            mesh.vertex_read_coordinate(coord, 0, mesh.loop_read_vertex(loop));
            soup.push(coord[0], coord[1], coord[2]);
            loop = mesh.loop_read_next(loop);
        } while (loop !== loop_start);
    }

    // append the bridge quads (two triangles each)
    for (let i = 0; i < bridges.length; i++) {
        const { a, b, pairing } = bridges[i];

        // corner0/corner1 are A's boundary edge; corner2/corner3 are B's, ordered so that the quad's
        // first triangle carries A's edge and the second carries B's edge (so both fuse onto their island)
        const b2x = pairing === 1 ? b.x2 : b.x1;
        const b2y = pairing === 1 ? b.y2 : b.y1;
        const b2z = pairing === 1 ? b.z2 : b.z1;

        const b3x = pairing === 1 ? b.x1 : b.x2;
        const b3y = pairing === 1 ? b.y1 : b.y2;
        const b3z = pairing === 1 ? b.z1 : b.z2;

        push_triangle_facing_up(
            soup,
            a.x1, a.y1, a.z1,
            a.x2, a.y2, a.z2,
            b2x, b2y, b2z,
            up_x, up_y, up_z
        );

        push_triangle_facing_up(
            soup,
            a.x1, a.y1, a.z1,
            b2x, b2y, b2z,
            b3x, b3y, b3z,
            up_x, up_y, up_z
        );
    }

    // rebuild topology from the augmented soup; the same merge/fuse the initial build uses re-establishes
    // connectivity, joining the bridge triangles onto the island boundaries whose vertices they share
    const rebuilt = new BinaryTopology();
    bt_mesh_from_unindexed_geometry(rebuilt, soup);
    bt_merge_verts_by_distance(rebuilt, VERTEX_MERGE_DISTANCE);
    bt_mesh_fuse_duplicate_edges(rebuilt);

    mesh.copy(rebuilt);

    return bridges.length;
}
