/**
 * # Local-frame ray ↔ primitive intersections (raycast narrowphase)
 *
 * Each function intersects a ray, **expressed in the shape's local frame**,
 * against a canonical primitive at the origin (sphere at the origin; box
 * axis-aligned spanning `[-h, +h]`; capsule along the local Y axis). They
 * return the hit distance `t` along the ray (`Infinity` on a miss) and write
 * the **local** outward surface normal (unit) into `outNormal[0..2]`.
 *
 * The ray-narrowphase dispatch transforms the world ray into the body's local
 * frame once (rotate by the inverse body rotation — a unit direction stays
 * unit, so `t` is preserved), calls the matching primitive, then rotates the
 * returned local normal back to world. Keeping the primitives canonical lets
 * the box and capsule tests be axis-aligned (cheap slab / cylinder math) and
 * shares a single transform across them.
 *
 * Conventions:
 *  - the ray direction is unit length (the caller guarantees it);
 *  - the first surface crossing at or after the origin within `tMax` is
 *    returned; a ray starting inside the shape returns its exit crossing;
 *  - the normal is the geometric outward surface normal at the hit.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

/**
 * Ray vs a sphere of radius `r` centred at the local origin.
 *
 * @param {Float64Array} outNormal length-3, written on hit
 * @param {number} ox @param {number} oy @param {number} oz  ray origin (local)
 * @param {number} dx @param {number} dy @param {number} dz  ray dir (local, unit)
 * @param {number} tMax
 * @param {number} r  sphere radius
 * @returns {number} hit distance, or `Infinity` on miss
 */
export function ray_sphere_local(outNormal, ox, oy, oz, dx, dy, dz, tMax, r) {
    // |o + t·d|² = r², d unit → t² + 2(o·d)t + (|o|² − r²) = 0.
    const b = ox * dx + oy * dy + oz * dz;
    const c = ox * ox + oy * oy + oz * oz - r * r;
    const disc = b * b - c;
    if (disc < 0) return Infinity;
    const sq = Math.sqrt(disc);
    let t = -b - sq;             // near root (entry)
    if (t < 0) t = -b + sq;      // origin inside the sphere → far root (exit)
    if (t < 0 || t > tMax) return Infinity;
    const inv = 1 / r;
    outNormal[0] = (ox + dx * t) * inv;
    outNormal[1] = (oy + dy * t) * inv;
    outNormal[2] = (oz + dz * t) * inv;
    return t;
}

/**
 * Ray vs an axis-aligned box spanning `[-hx,hx] × [-hy,hy] × [-hz,hz]` at the
 * local origin (the canonical pose of {@link BoxShape3D}). Slab method, with
 * the entry (or, origin-inside, exit) face's outward normal.
 *
 * @param {Float64Array} outNormal length-3, written on hit
 * @param {number} ox @param {number} oy @param {number} oz  ray origin (local)
 * @param {number} dx @param {number} dy @param {number} dz  ray dir (local, unit)
 * @param {number} tMax
 * @param {number} hx @param {number} hy @param {number} hz  half-extents
 * @returns {number} hit distance, or `Infinity` on miss
 */
export function ray_box_local(outNormal, ox, oy, oz, dx, dy, dz, tMax, hx, hy, hz) {
    let tmin = -Infinity, tmax = Infinity;
    let enterAxis = 0, enterSign = 0, exitAxis = 0, exitSign = 0;

    // X slab.
    if (dx !== 0) {
        const inv = 1 / dx;
        let tn = (-hx - ox) * inv, tf = (hx - ox) * inv, nsign = -1;
        if (tn > tf) { const tmp = tn; tn = tf; tf = tmp; nsign = 1; }
        if (tn > tmin) { tmin = tn; enterAxis = 0; enterSign = nsign; }
        if (tf < tmax) { tmax = tf; exitAxis = 0; exitSign = -nsign; }
    } else if (ox < -hx || ox > hx) {
        return Infinity;
    }
    // Y slab.
    if (dy !== 0) {
        const inv = 1 / dy;
        let tn = (-hy - oy) * inv, tf = (hy - oy) * inv, nsign = -1;
        if (tn > tf) { const tmp = tn; tn = tf; tf = tmp; nsign = 1; }
        if (tn > tmin) { tmin = tn; enterAxis = 1; enterSign = nsign; }
        if (tf < tmax) { tmax = tf; exitAxis = 1; exitSign = -nsign; }
    } else if (oy < -hy || oy > hy) {
        return Infinity;
    }
    // Z slab.
    if (dz !== 0) {
        const inv = 1 / dz;
        let tn = (-hz - oz) * inv, tf = (hz - oz) * inv, nsign = -1;
        if (tn > tf) { const tmp = tn; tn = tf; tf = tmp; nsign = 1; }
        if (tn > tmin) { tmin = tn; enterAxis = 2; enterSign = nsign; }
        if (tf < tmax) { tmax = tf; exitAxis = 2; exitSign = -nsign; }
    } else if (oz < -hz || oz > hz) {
        return Infinity;
    }

    if (tmax < tmin || tmax < 0) return Infinity;

    let t, axis, sign;
    if (tmin >= 0) { t = tmin; axis = enterAxis; sign = enterSign; }
    else { t = tmax; axis = exitAxis; sign = exitSign; } // origin inside the box

    if (t > tMax) return Infinity;
    outNormal[0] = 0; outNormal[1] = 0; outNormal[2] = 0;
    outNormal[axis] = sign;
    return t;
}

/**
 * Ray vs a capsule along the local Y axis: a cylinder of radius `r` over
 * `y ∈ [−hh, hh]` capped by hemispheres of radius `r` at `(0, ±hh, 0)` — the
 * canonical pose of {@link CapsuleShape3D} (`hh = height/2`). Tests the
 * infinite cylinder (clamped to the segment) and the two cap spheres, taking
 * the nearest valid crossing.
 *
 * @param {Float64Array} outNormal length-3, written on hit
 * @param {number} ox @param {number} oy @param {number} oz  ray origin (local)
 * @param {number} dx @param {number} dy @param {number} dz  ray dir (local, unit)
 * @param {number} tMax
 * @param {number} r   capsule radius
 * @param {number} hh  half-height of the cylindrical section (`height/2`)
 * @returns {number} hit distance, or `Infinity` on miss
 */
export function ray_capsule_local(outNormal, ox, oy, oz, dx, dy, dz, tMax, r, hh) {
    let best = Infinity;
    let nx = 0, ny = 0, nz = 0;
    const r2 = r * r;
    const inv_r = 1 / r;

    // --- Cylinder side (infinite cylinder about Y, projected to XZ). ---
    const a = dx * dx + dz * dz;
    if (a > 1e-12) {
        const b = ox * dx + oz * dz;
        const c = ox * ox + oz * oz - r2;
        const disc = b * b - a * c;
        if (disc >= 0) {
            const sq = Math.sqrt(disc);
            const inv_a = 1 / a;
            // Both roots; the smaller non-negative one whose hit lies on the
            // cylindrical section is the side contact.
            const roots = (-b - sq) * inv_a;
            const rootf = (-b + sq) * inv_a;
            for (let i = 0; i < 2; i++) {
                const t = i === 0 ? roots : rootf;
                if (t < 0 || t >= best || t > tMax) continue;
                const y = oy + dy * t;
                if (y < -hh || y > hh) continue; // off the cylinder, onto a cap
                best = t;
                const px = ox + dx * t, pz = oz + dz * t;
                nx = px * inv_r; ny = 0; nz = pz * inv_r;
                break; // roots is the nearer; if it qualified we're done
            }
        }
    }

    // --- Cap spheres at (0, ±hh, 0), each valid only on its outer hemisphere. ---
    for (let cap = 0; cap < 2; cap++) {
        const cy = cap === 0 ? hh : -hh;
        const oyc = oy - cy;
        const bb = ox * dx + oyc * dy + oz * dz;
        const cc = ox * ox + oyc * oyc + oz * oz - r2;
        const disc = bb * bb - cc;
        if (disc < 0) continue;
        const sq = Math.sqrt(disc);
        const tn = -bb - sq, tf = -bb + sq;
        for (let i = 0; i < 2; i++) {
            const t = i === 0 ? tn : tf;
            if (t < 0 || t >= best || t > tMax) continue;
            const hy = oy + dy * t;
            // Only the hemisphere beyond the segment end belongs to this cap.
            if (cap === 0 ? hy < hh : hy > -hh) continue;
            best = t;
            nx = (ox + dx * t) * inv_r;
            ny = (hy - cy) * inv_r;
            nz = (oz + dz * t) * inv_r;
            break;
        }
    }

    if (best === Infinity) return Infinity;
    outNormal[0] = nx; outNormal[1] = ny; outNormal[2] = nz;
    return best;
}
