import { assert } from "../../../assert.js";
import { clamp } from "../../../math/clamp.js";
import { v3_length } from "../../vec3/v3_length.js";
import { Vector3 } from "../../Vector3.js";
import { AbstractShape3D } from "./AbstractShape3D.js";

/**
 * Heightmap shape, intended primarily for terrain.
 *
 * The shape is a closed solid bounded below by the plane perpendicular to
 * {@link orientation} (the "floor") and above by a height-field surface
 * defined by a {@link Sampler2D}. Heights are sampled with Catmull-Rom
 * filtering ({@link Sampler2D#sampleChannelCatmullRomUV}) — the *same* cubic
 * the terrain renderer uses (`sampleChannelBicubicUV` expands to the
 * identical Catmull-Rom weights, with the matching `u·width − 0.5` UV
 * convention), so the collision and render surfaces coincide at every point
 * they both sample.
 *
 * They differ only in *tessellation density*: the renderer lays out
 * `size × resolution` segments per side, while collision defaults to one
 * quad per sampler cell. {@link tessellation} closes that gap — set it > 1
 * to subdivide each sampler cell into N×N sub-cells so the contact surface
 * approaches render fidelity even when the sampler is deliberately coarse.
 *
 * Local frame layout:
 *   - The orientation vector defines the local "up" axis (unit).
 *   - An orthonormal basis (u, v, n=orientation) is built from it.
 *   - Footprint extends along the basis-u axis over [-size.x/2, +size.x/2]
 *     and along the basis-v axis over [-size.z/2, +size.z/2].
 *   - The surface height (along orientation) at heightmap-UV (u01, v01) is
 *     `sampler.sampleChannelCatmullRomUV(u01, v01, 0)`.
 *   - The solid volume occupies `h ∈ [0, sampledHeight(u01, v01)]` along the
 *     orientation axis, with `0` being the floor at body-local origin.
 *   - `size.y` is the maximum height value of the heightfield (used for the
 *     bounding box). Sampler values are NOT clamped to it.
 *
 * NON-CONVEX. {@link support} throws — GJK/EPA cannot be run against a
 * heightmap directly. The physics narrowphase must dispatch a dedicated
 * grid-traversal path when one of the colliders is a heightmap.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */
export class HeightMapShape3D extends AbstractShape3D {
    constructor() {
        super();

        /**
         * Unit vector defining the local "up" axis (the direction the
         * heightmap's surface faces). Default is +Y.
         * @readonly
         * @type {Vector3}
         */
        this.orientation = new Vector3(0, 1, 0);

        /**
         * Bounding-box extents in the heightmap-local (u, height, v) frame.
         *   size.x — footprint extent along basis-u (perpendicular to orientation)
         *   size.y — maximum heightmap height (extent along orientation)
         *   size.z — footprint extent along basis-v (perpendicular to orientation)
         * @readonly
         * @type {Vector3}
         */
        this.size = new Vector3(1, 1, 1);

        /**
         * Sampler holding height values. Float32 backing recommended so the
         * Terrain system's height texture plugs in directly. Single-channel
         * sampler is the common case; only channel 0 is read.
         * @type {Sampler2D | null}
         */
        this.sampler = null;

        /**
         * Collision tessellation factor: the narrowphase splits each sampler
         * cell into `tessellation × tessellation` sub-cells before emitting
         * collision triangles, sampling the same Catmull-Rom filter at the
         * finer sub-cell corners. `1` (the default) is one quad per sampler
         * cell — the legacy behaviour. Larger values let a coarse sampler's
         * contact surface approach the rendered mesh's fidelity.
         *
         * Non-negative integer (validated by {@link HeightMapShape3D.from});
         * cost is O(N²) per cell, so the caller owns the fidelity/cost
         * trade-off. `0` yields no collision triangles — the degenerate empty
         * case, mirroring a zero footprint size. Affects only triangle
         * enumeration — the continuous queries ({@link sample_height_at_uv},
         * {@link signed_distance_at_point}, {@link nearest_point_on_surface})
         * read the smooth filter directly and are unaffected.
         * @type {number}
         */
        this.tessellation = 1;

        /**
         * Cached orthonormal basis [u_x,u_y,u_z, v_x,v_y,v_z, n_x,n_y,n_z]
         * built from {@link orientation}. Updated lazily by {@link _ensure_basis}.
         * @private
         * @type {Float64Array}
         */
        this._basis = new Float64Array(9);

        // last-seen orientation components, used to detect dirty basis
        this._basis_orientation_x = NaN;
        this._basis_orientation_y = NaN;
        this._basis_orientation_z = NaN;
    }

    /**
     * Convenience constructor.
     * @param {Sampler2D} sampler
     * @param {number} size_x footprint extent along basis-u
     * @param {number} size_y maximum heightmap height (along orientation)
     * @param {number} size_z footprint extent along basis-v
     * @param {Vector3} [orientation] defaults to +Y
     * @param {number} [tessellation] collision sub-cells per sampler cell per
     *   axis; non-negative integer, defaults to 1 (one quad per sampler cell).
     *   See {@link HeightMapShape3D#tessellation}.
     * @returns {HeightMapShape3D}
     */
    static from(sampler, size_x, size_y, size_z, orientation, tessellation = 1) {
        assert.isNumber(size_x, "size_x");
        assert.isNumber(size_y, "size_y");
        assert.isNumber(size_z, "size_z");
        assert.greaterThanOrEqual(size_x, 0, "size_x");
        assert.greaterThanOrEqual(size_y, 0, "size_y");
        assert.greaterThanOrEqual(size_z, 0, "size_z");
        assert.isNonNegativeInteger(tessellation, "tessellation");

        const r = new HeightMapShape3D();
        r.sampler = sampler;
        r.size.set(size_x, size_y, size_z);
        r.tessellation = tessellation;

        if (orientation !== undefined) {
            r.orientation.set(orientation.x, orientation.y, orientation.z);
        }

        return r;
    }

    /**
     * Recompute the orthonormal basis (u, v, n) if {@link orientation} changed.
     *
     * Construction chosen so that the default orientation +Y produces the
     * intuitive mapping `u = +X, v = +Z, n = +Y` — i.e. size.x runs along
     * body X, size.z along body Z. We project body +X onto the plane
     * perpendicular to n (the orientation), with a fall-back to projecting
     * body +Z when n is too close to colinear with +X.
     * @private
     */
    _ensure_basis() {
        const nx = this.orientation[0];
        const ny = this.orientation[1];
        const nz = this.orientation[2];

        if (
            nx === this._basis_orientation_x
            && ny === this._basis_orientation_y
            && nz === this._basis_orientation_z
        ) {
            return;
        }

        // tangent u = normalize(project body +X onto plane perpendicular to n)
        // fall back to body +Z if n is too colinear with +X
        let u_x, u_y, u_z;

        if (Math.abs(nx) < 0.9) {
            // u = (+X) - (n . +X) * n  =  (1 - nx*nx, -nx*ny, -nx*nz)
            u_x = 1 - nx * nx;
            u_y = -nx * ny;
            u_z = -nx * nz;
        } else {
            // u = (+Z) - (n . +Z) * n  =  (-nz*nx, -nz*ny, 1 - nz*nz)
            u_x = -nz * nx;
            u_y = -nz * ny;
            u_z = 1 - nz * nz;
        }

        const u_inv = 1 / v3_length(u_x, u_y, u_z);
        u_x *= u_inv;
        u_y *= u_inv;
        u_z *= u_inv;

        // v = u × n  (for n=+Y, u=+X this gives v=+Z, the intuitive choice)
        const v_x = u_y * nz - u_z * ny;
        const v_y = u_z * nx - u_x * nz;
        const v_z = u_x * ny - u_y * nx;

        const b = this._basis;
        b[0] = u_x; b[1] = u_y; b[2] = u_z;
        b[3] = v_x; b[4] = v_y; b[5] = v_z;
        b[6] = nx;  b[7] = ny;  b[8] = nz;

        this._basis_orientation_x = nx;
        this._basis_orientation_y = ny;
        this._basis_orientation_z = nz;
    }

    /**
     * Sample the surface height at heightmap UV coordinates.
     * Uses Catmull-Rom filtering to match the terrain system's geometry construction.
     * @param {number} u01 horizontal UV, in [0, 1]
     * @param {number} v01 vertical UV, in [0, 1]
     * @returns {number} height along orientation axis
     */
    sample_height_at_uv(u01, v01) {
        return this.sampler.sampleChannelCatmullRomUV(u01, v01, 0);
    }

    /**
     * Project a body-local position to the surface and sample the height there.
     * The returned height is in the orientation-axis direction.
     * Positions outside the footprint sample the clamped UV (sampler clamps).
     * @param {number} px body-local x
     * @param {number} py body-local y
     * @param {number} pz body-local z
     * @returns {number}
     */
    sample_height_at_position(px, py, pz) {
        this._ensure_basis();

        const b = this._basis;

        // project onto basis u and basis v
        const u_coord = b[0] * px + b[1] * py + b[2] * pz;
        const v_coord = b[3] * px + b[4] * py + b[5] * pz;

        const u01 = u_coord / this.size[0] + 0.5;
        const v01 = v_coord / this.size[2] + 0.5;

        return this.sample_height_at_uv(u01, v01);
    }

    compute_bounding_box(result) {
        this._ensure_basis();

        const b = this._basis;

        const sx = this.size[0];
        const sy = this.size[1];
        const sz = this.size[2];

        const half_u = sx * 0.5;
        const half_v = sz * 0.5;

        // For each body axis k (0=x, 1=y, 2=z):
        //   body[k] = b[0+k]*u + b[3+k]*v + b[6+k]*h
        //
        // The (u,v) footprint contribution is symmetric (u ∈ [-half_u, +half_u], v ∈ [-half_v, +half_v])
        // The height contribution is asymmetric (h ∈ [0, sy])
        for (let k = 0; k < 3; k++) {
            const cu = b[k];
            const cv = b[3 + k];
            const ch = b[6 + k];

            const uv_extent = Math.abs(cu) * half_u + Math.abs(cv) * half_v;

            const h_lo = ch < 0 ? ch * sy : 0;
            const h_hi = ch > 0 ? ch * sy : 0;

            result[k] = -uv_extent + h_lo;
            result[k + 3] = uv_extent + h_hi;
        }
    }

    contains_point(point) {
        const px = point[0];
        const py = point[1];
        const pz = point[2];

        this._ensure_basis();

        const b = this._basis;

        // project into heightmap-local frame (u, v, h)
        const u_coord = b[0] * px + b[1] * py + b[2] * pz;
        const v_coord = b[3] * px + b[4] * py + b[5] * pz;
        const h_coord = b[6] * px + b[7] * py + b[8] * pz;

        const half_u = this.size[0] * 0.5;
        const half_v = this.size[2] * 0.5;

        if (u_coord <= -half_u || u_coord >= half_u) return false;
        if (v_coord <= -half_v || v_coord >= half_v) return false;
        if (h_coord <= 0 || h_coord >= this.size[1]) return false;

        const u01 = u_coord / this.size[0] + 0.5;
        const v01 = v_coord / this.size[2] + 0.5;

        const surface_h = this.sample_height_at_uv(u01, v01);

        return h_coord < surface_h;
    }

    /**
     * Approximate signed distance: difference between the point's height
     * (along orientation) and the surface height sampled at the point's
     * footprint UV. POSITIVE = above surface, NEGATIVE = below.
     *
     * Locally correct when the surface is flat; biased when the surface
     * has significant slope. Sufficient for cling/raycast queries that
     * walk down to the surface.
     */
    signed_distance_at_point(point) {
        const px = point[0];
        const py = point[1];
        const pz = point[2];

        this._ensure_basis();

        const b = this._basis;

        const u_coord = b[0] * px + b[1] * py + b[2] * pz;
        const v_coord = b[3] * px + b[4] * py + b[5] * pz;
        const h_coord = b[6] * px + b[7] * py + b[8] * pz;

        const u01 = u_coord / this.size[0] + 0.5;
        const v01 = v_coord / this.size[2] + 0.5;

        const surface_h = this.sample_height_at_uv(u01, v01);

        return h_coord - surface_h;
    }

    /**
     * Project a reference point onto the surface along the orientation axis.
     * The footprint UV is clamped, so points outside the footprint produce
     * the nearest edge-of-footprint surface sample (approximate).
     */
    nearest_point_on_surface(result, reference) {
        const rx = reference[0];
        const ry = reference[1];
        const rz = reference[2];

        this._ensure_basis();

        const b = this._basis;

        const u_coord = b[0] * rx + b[1] * ry + b[2] * rz;
        const v_coord = b[3] * rx + b[4] * ry + b[5] * rz;

        const half_u = this.size[0] * 0.5;
        const half_v = this.size[2] * 0.5;

        const u_clamped = clamp(u_coord, -half_u, half_u);
        const v_clamped = clamp(v_coord, -half_v, half_v);

        const u01 = u_clamped / this.size[0] + 0.5;
        const v01 = v_clamped / this.size[2] + 0.5;

        const surface_h = this.sample_height_at_uv(u01, v01);

        // compose body-local point: u_axis*u_clamped + v_axis*v_clamped + n_axis*surface_h
        result[0] = b[0] * u_clamped + b[3] * v_clamped + b[6] * surface_h;
        result[1] = b[1] * u_clamped + b[4] * v_clamped + b[7] * surface_h;
        result[2] = b[2] * u_clamped + b[5] * v_clamped + b[8] * surface_h;
    }

    /**
     * Heightmaps are non-convex; GJK/EPA cannot work against them directly.
     * The physics narrowphase must dispatch a grid-traversal path that
     * decomposes the heightmap into per-cell triangle pairs and tests each
     * against the other shape (analogous to Bullet's btHeightfieldTerrainShape
     * × btConcaveShape interface).
     *
     * This throws rather than returning a degenerate result so the call
     * site is forced to handle heightmaps explicitly.
     */
    support(result, result_offset, direction_x, direction_y, direction_z) {
        throw new Error("HeightMapShape3D.support: heightmaps are non-convex; the narrowphase must dispatch grid-traversal instead.");
    }

    sample_random_point_in_volume(result, result_offset, random) {
        const u01 = random();
        const v01 = random();

        const u_coord = (u01 - 0.5) * this.size[0];
        const v_coord = (v01 - 0.5) * this.size[2];

        const surface_h = this.sample_height_at_uv(u01, v01);
        const h_coord = random() * surface_h;

        this._ensure_basis();

        const b = this._basis;

        result[result_offset]     = b[0] * u_coord + b[3] * v_coord + b[6] * h_coord;
        result[result_offset + 1] = b[1] * u_coord + b[4] * v_coord + b[7] * h_coord;
        result[result_offset + 2] = b[2] * u_coord + b[5] * v_coord + b[8] * h_coord;
    }

    /**
     * Sum of sampler heights × per-cell footprint area. This is the
     * piecewise-constant approximation of the integral ∫h(u,v) dA over
     * the footprint — exact when h is constant per cell, biased when
     * h is smooth.
     */
    get volume() {
        const sampler = this.sampler;

        if (sampler === null) {
            return 0;
        }

        const w = sampler.width;
        const h = sampler.height;

        if (w === 0 || h === 0) {
            return 0;
        }

        const cell_area = (this.size[0] / w) * (this.size[2] / h);

        let total = 0;

        for (let y = 0; y < h; y++) {
            for (let x = 0; x < w; x++) {
                total += sampler.readChannel(x, y, 0);
            }
        }

        return total * cell_area;
    }

    /**
     * Returns just the footprint area. A true heightmap surface area
     * requires integrating sqrt(1 + (∂h/∂u)² + (∂h/∂v)²) over the grid;
     * the footprint area is a lower bound and is sufficient for the
     * physics inertia-tensor seam (heightmaps are static anyway).
     */
    get surface_area() {
        return this.size[0] * this.size[2];
    }

    /**
     * @param {HeightMapShape3D} other
     * @returns {boolean}
     */
    equals(other) {
        if (!super.equals(other)) return false;

        if (!this.orientation.equals(other.orientation)) return false;
        if (!this.size.equals(other.size)) return false;
        if (this.tessellation !== other.tessellation) return false;

        // strict identity is enough for sampler equality in the common case;
        // fall through to value equality so two shapes built from independent
        // but identical sampler instances still compare equal
        if (this.sampler === other.sampler) return true;

        if (this.sampler === null || other.sampler === null) return false;

        return this.sampler.equals(other.sampler);
    }

    hash() {
        const a = this.orientation.hash();
        const b = this.size.hash();
        const c = this.sampler !== null ? this.sampler.hash() : 0;

        let h = (a * 31 + b) | 0;
        h = (h * 31 + c) | 0;
        h = (h * 31 + this.tessellation) | 0;

        return h;
    }
}

/**
 * Fast type-check marker, matching the pattern on every other concrete
 * AbstractShape3D subclass. The physics narrowphase reads this to dispatch
 * the heightmap-vs-X grid-traversal path.
 * @readonly
 * @type {boolean}
 */
HeightMapShape3D.prototype.isHeightMapShape3D = true;

/**
 * Heightmaps are non-convex: the solid volume bounded by an arbitrary
 * height-field has valleys and overhangs that break GJK's convex-Minkowski
 * precondition. The narrowphase must use grid traversal + per-triangle
 * GJK instead of feeding this shape's {@link support} into pair tests.
 * @readonly
 * @type {boolean}
 */
HeightMapShape3D.prototype.is_convex = false;
