import { Ray3 } from "../../../core/geom/3d/ray/Ray3.js";
import { PhysicsSurfacePoint } from "../queries/PhysicsSurfacePoint.js";
import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
import { v3_length } from "../../../core/geom/vec3/v3_length.js";
import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
import { BodyKind } from "../ecs/BodyKind.js";

/**
 * # Raycast vehicle controller
 *
 * The vehicle model most games ship: a single rigid-body **chassis** plus a set
 * of **raycast wheels**. Each wheel is not a body — it is a downward ray from a
 * chassis-local mount point. Every frame, for each wheel, the controller:
 *
 *   1. casts the suspension ray and, on a ground hit, measures the spring
 *      compression;
 *   2. applies a spring + damper **suspension force** along the contact normal
 *      (this is what holds the chassis up);
 *   3. applies **tyre friction** at the contact patch — a lateral grip impulse
 *      that resists sideways slide, plus a longitudinal drive / brake impulse,
 *      the two clamped together to a friction circle `μ·N`.
 *
 * Steering rotates the steered wheels' forward direction; a drive force is split
 * across the driven wheels. It is a *controller on top of the public physics
 * API* (`raycast` + `applyForceAt` + `applyImpulseAt`), not a new constraint —
 * call {@link update} once per frame **before** `PhysicsSystem.fixedUpdate`, so
 * the suspension force it accumulates is integrated by that step and the
 * friction impulses act on the current velocity.
 *
 * The 6-DOF joint (spring linear + motor angular) is the alternative
 * "simulated wheel" path; this raycast controller is the lighter, more robust
 * default. Suspension-ray accuracy follows `PhysicsSystem.raycast`, which is
 * narrowphase-exact for sphere / box / capsule / mesh / heightmap ground.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

const scratch_v = new Float64Array(3);
const scratch_i = new Float64Array(3);

/**
 * Effective mass denominator for a unit-direction velocity constraint at a
 * body-relative point: `invM + (r×d)·Iw⁻¹·(r×d)`.
 * @returns {number}
 */
function effective_mass(rb, transform, invM, rx, ry, rz, dx, dy, dz) {
    let k = invM;
    const ii = rb.inverseInertiaLocal;
    if (ii.x !== 0 || ii.y !== 0 || ii.z !== 0) {
        const cx = ry * dz - rz * dy;
        const cy = rz * dx - rx * dz;
        const cz = rx * dy - ry * dx;
        world_inverse_inertia_apply(scratch_i, 0, ii, transform.rotation, cx, cy, cz);
        k += cx * scratch_i[0] + cy * scratch_i[1] + cz * scratch_i[2];
    }
    return k;
}

/** Rotate vector `(vx,vy,vz)` about unit axis `(ux,uy,uz)` by `angle`, into `out`. */
function rotate_about(out, vx, vy, vz, ux, uy, uz, angle) {
    const c = Math.cos(angle), s = Math.sin(angle);
    const dot = ux * vx + uy * vy + uz * vz;
    // v·c + (k×v)·s + k·(k·v)·(1−c)
    const kxx = uy * vz - uz * vy;
    const kxy = uz * vx - ux * vz;
    const kxz = ux * vy - uy * vx;
    out[0] = vx * c + kxx * s + ux * dot * (1 - c);
    out[1] = vy * c + kxy * s + uy * dot * (1 - c);
    out[2] = vz * c + kxz * s + uz * dot * (1 - c);
}


/**
 * One wheel's config + per-frame runtime state. The runtime fields
 * (`inContact`, `compression`, contact point/normal, `rotation`, …) are read by
 * rendering / gameplay; do not write them.
 */
export class Wheel {
    constructor() {
        // --- config ---
        /** Mount point on the chassis, local. @type {Float64Array} */
        this.localPosition = new Float64Array(3);
        /** Suspension direction (local, unit, points "down"). @type {Float64Array} */
        this.localSuspensionDir = Float64Array.from([0, -1, 0]);
        /** Forward / rolling direction (local, unit). @type {Float64Array} */
        this.localForward = Float64Array.from([0, 0, 1]);
        this.suspensionRestLength = 0.5;
        this.suspensionStiffness = 100;
        this.suspensionDamping = 10;
        this.suspensionMaxForce = Infinity;
        this.radius = 0.3;
        this.friction = 1.2;
        this.steered = false;
        this.driven = false;

        // --- runtime ---
        this.inContact = false;
        this.suspensionLength = this.suspensionRestLength;
        this.compression = 0;
        /** World contact point. @type {Float64Array} */
        this.contactPoint = new Float64Array(3);
        /** World contact normal. @type {Float64Array} */
        this.contactNormal = Float64Array.from([0, 1, 0]);
        /** Body id of the surface under the wheel, or −1. */
        this.contactBodyId = -1;
        /** Current steering angle (rad), set via {@link RaycastVehicle#setSteering}. */
        this.steering = 0;
        /** Accumulated spin angle (rad) for rendering. */
        this.rotation = 0;
        /** Forward ground speed at the contact (m/s). */
        this.forwardSpeed = 0;
        /** Suspension force magnitude applied this frame (N). */
        this.suspensionForce = 0;
    }
}

export class RaycastVehicle {
    /**
     * @param {PhysicsSystem} system
     * @param {RigidBody} chassisBody  the chassis rigid body (Dynamic)
     * @param {Transform} chassisTransform  its world transform
     */
    constructor(system, chassisBody, chassisTransform) {
        this.system = system;
        this.chassisBody = chassisBody;
        this.chassisTransform = chassisTransform;

        /** @type {Wheel[]} */
        this.wheels = [];

        this.__driveForce = 0;
        this.__brakeForce = 0;
        this.__drivenCount = 0;

        this.__ray = new Ray3();
        this.__hit = new PhysicsSurfacePoint();
        // Exclude the chassis' own colliders from the suspension raycast.
        this.__filter = (entity, collider) => collider._bodyId !== this.chassisBody._bodyId;

        // Reused force / impulse / point payloads for the public API calls.
        this.__force = { x: 0, y: 0, z: 0 };
        this.__point = { x: 0, y: 0, z: 0 };
    }

    /**
     * Add a wheel. Returns the created {@link Wheel} (also pushed to
     * {@link wheels}); set any further config fields on it before stepping.
     *
     * @param {object} opts
     * @param {number[]} opts.localPosition  mount point on the chassis, local.
     * @param {number[]} [opts.localSuspensionDir]  unit "down", default (0,−1,0).
     * @param {number[]} [opts.localForward]  unit rolling dir, default (0,0,1).
     * @param {number} [opts.suspensionRestLength]
     * @param {number} [opts.suspensionStiffness]  N/m.
     * @param {number} [opts.suspensionDamping]  N·s/m.
     * @param {number} [opts.suspensionMaxForce]  N (clamp).
     * @param {number} [opts.radius]
     * @param {number} [opts.friction]  tyre friction coefficient μ.
     * @param {boolean} [opts.steered]
     * @param {boolean} [opts.driven]
     * @returns {Wheel}
     */
    addWheel(opts) {
        const w = new Wheel();
        const p = opts.localPosition;
        w.localPosition[0] = p[0]; w.localPosition[1] = p[1]; w.localPosition[2] = p[2];
        if (opts.localSuspensionDir) {
            const d = opts.localSuspensionDir, L = v3_length(d[0], d[1], d[2]) || 1;
            w.localSuspensionDir[0] = d[0] / L; w.localSuspensionDir[1] = d[1] / L; w.localSuspensionDir[2] = d[2] / L;
        }
        if (opts.localForward) {
            const f = opts.localForward, L = v3_length(f[0], f[1], f[2]) || 1;
            w.localForward[0] = f[0] / L; w.localForward[1] = f[1] / L; w.localForward[2] = f[2] / L;
        }
        if (opts.suspensionRestLength !== undefined) w.suspensionRestLength = opts.suspensionRestLength;
        if (opts.suspensionStiffness !== undefined) w.suspensionStiffness = opts.suspensionStiffness;
        if (opts.suspensionDamping !== undefined) w.suspensionDamping = opts.suspensionDamping;
        if (opts.suspensionMaxForce !== undefined) w.suspensionMaxForce = opts.suspensionMaxForce;
        if (opts.radius !== undefined) w.radius = opts.radius;
        if (opts.friction !== undefined) w.friction = opts.friction;
        if (opts.steered !== undefined) w.steered = opts.steered;
        if (opts.driven !== undefined) w.driven = opts.driven;
        w.suspensionLength = w.suspensionRestLength;
        this.wheels.push(w);
        if (w.driven) this.__drivenCount++;
        return w;
    }

    /** Set the steering angle (rad) on every steered wheel. */
    setSteering(angle) {
        for (let i = 0; i < this.wheels.length; i++) {
            if (this.wheels[i].steered) this.wheels[i].steering = angle;
        }
    }

    /** Total engine drive force (N), split evenly across the driven wheels. */
    setDriveForce(force) { this.__driveForce = force; }

    /** Brake force (N) per wheel, opposing each wheel's forward motion. */
    setBrake(force) { this.__brakeForce = force; }

    /**
     * Step the vehicle: cast suspension rays, apply suspension + tyre forces.
     * Call once per frame **before** `PhysicsSystem.fixedUpdate(dt)` with the
     * same `dt`.
     * @param {number} dt
     */
    update(dt) {
        const rb = this.chassisBody;
        if (rb.kind !== BodyKind.Dynamic || dt <= 0) return;
        const tr = this.chassisTransform;
        const invMass = rb.mass > 0 ? 1 / rb.mass : 0;
        const q = tr.rotation;
        const px = tr.position.x, py = tr.position.y, pz = tr.position.z;
        const lv = rb.linearVelocity, av = rb.angularVelocity;
        const drivePer = this.__drivenCount > 0 ? this.__driveForce / this.__drivenCount : 0;

        for (let wi = 0; wi < this.wheels.length; wi++) {
            const w = this.wheels[wi];

            // World mount point and suspension axis.
            v3_quat3_apply(scratch_v, 0, w.localPosition[0], w.localPosition[1], w.localPosition[2], q[0], q[1], q[2], q[3]);
            const mx = px + scratch_v[0], my = py + scratch_v[1], mz = pz + scratch_v[2];
            v3_quat3_apply(scratch_v, 0, w.localSuspensionDir[0], w.localSuspensionDir[1], w.localSuspensionDir[2], q[0], q[1], q[2], q[3]);
            const sdx = scratch_v[0], sdy = scratch_v[1], sdz = scratch_v[2]; // suspension down (unit)

            const maxLen = w.suspensionRestLength + w.radius;
            this.__ray.setOrigin(mx, my, mz);
            this.__ray.setDirection(sdx, sdy, sdz);
            this.__ray.tMax = maxLen;

            const hit = this.system.raycast(this.__ray, this.__hit, this.__filter);
            // `raycast` already bounds the hit to ray.tMax (= maxLen) and only
            // returns true for t strictly < maxLen, so `__hit.t > maxLen` was dead.
            if (!hit) {
                // Airborne: suspension fully extended, no force. Reset ALL the
                // contact-frame fields — including contactBodyId and forwardSpeed,
                // which otherwise retain stale last-grounded-frame values.
                w.inContact = false;
                w.suspensionForce = 0;
                w.suspensionLength = w.suspensionRestLength;
                w.compression = 0;
                w.contactBodyId = -1; // documented no-contact sentinel
                w.forwardSpeed = 0;
                continue;
            }

            const t = this.__hit.t;
            w.inContact = true;
            w.contactBodyId = this.__hit.body_id;
            w.suspensionLength = t - w.radius;
            if (w.suspensionLength < 0) w.suspensionLength = 0;
            w.compression = w.suspensionRestLength - w.suspensionLength;

            // Contact point along the ray; normal from the hit.
            const cx = mx + sdx * t, cy = my + sdy * t, cz = mz + sdz * t;
            let nx = this.__hit.normal.x, ny = this.__hit.normal.y, nz = this.__hit.normal.z;
            const nlen = v3_length(nx, ny, nz) || 1;
            nx /= nlen; ny /= nlen; nz /= nlen;
            w.contactPoint[0] = cx; w.contactPoint[1] = cy; w.contactPoint[2] = cz;
            w.contactNormal[0] = nx; w.contactNormal[1] = ny; w.contactNormal[2] = nz;

            // Velocity of the chassis at the contact point.
            const rx = cx - px, ry = cy - py, rz = cz - pz;
            const vptx = lv.x + av.y * rz - av.z * ry;
            const vpty = lv.y + av.z * rx - av.x * rz;
            const vptz = lv.z + av.x * ry - av.y * rx;

            // --- Suspension: spring + damper along the contact normal ---
            const projVel = vptx * nx + vpty * ny + vptz * nz; // closing speed along normal
            let susForce = w.suspensionStiffness * w.compression - w.suspensionDamping * projVel;
            if (susForce < 0) susForce = 0;
            else if (susForce > w.suspensionMaxForce) susForce = w.suspensionMaxForce;
            w.suspensionForce = susForce;

            if (susForce !== 0) {
                this.__force.x = nx * susForce; this.__force.y = ny * susForce; this.__force.z = nz * susForce;
                this.__point.x = cx; this.__point.y = cy; this.__point.z = cz;
                this.system.applyForceAt(rb, tr, this.__force, this.__point);
            }

            // --- Tyre friction: forward / side basis in the contact plane ---
            v3_quat3_apply(scratch_v, 0, w.localForward[0], w.localForward[1], w.localForward[2], q[0], q[1], q[2], q[3]);
            let fwx = scratch_v[0], fwy = scratch_v[1], fwz = scratch_v[2];
            if (w.steering !== 0) {
                // Steer about the suspension-up axis (−suspension dir).
                rotate_about(scratch_v, fwx, fwy, fwz, -sdx, -sdy, -sdz, w.steering);
                fwx = scratch_v[0]; fwy = scratch_v[1]; fwz = scratch_v[2];
            }
            // Project forward onto the contact plane, normalise.
            let fdotn = fwx * nx + fwy * ny + fwz * nz;
            fwx -= fdotn * nx; fwy -= fdotn * ny; fwz -= fdotn * nz;
            let fl = v3_length(fwx, fwy, fwz);
            if (fl < 1e-6) continue; // degenerate (forward parallel to normal)
            fwx /= fl; fwy /= fl; fwz /= fl;
            // Side = normal × forward (already unit & in-plane).
            const sx = ny * fwz - nz * fwy;
            const sy = nz * fwx - nx * fwz;
            const sz = nx * fwy - ny * fwx;

            const fwdVel = vptx * fwx + vpty * fwy + vptz * fwz;
            const sideVel = vptx * sx + vpty * sy + vptz * sz;
            w.forwardSpeed = fwdVel;

            // Lateral grip: impulse that cancels the side velocity.
            const denomSide = effective_mass(rb, tr, invMass, rx, ry, rz, sx, sy, sz);
            let jSide = denomSide > 0 ? -sideVel / denomSide : 0;

            // Longitudinal: engine drive minus brake (brake never reverses).
            let jFwd = w.driven ? drivePer * dt : 0;
            if (this.__brakeForce > 0 && fwdVel !== 0) {
                const denomFwd = effective_mass(rb, tr, invMass, rx, ry, rz, fwx, fwy, fwz);
                const stopImpulse = denomFwd > 0 ? Math.abs(fwdVel) / denomFwd : 0;
                const brakeImpulse = Math.min(this.__brakeForce * dt, stopImpulse);
                jFwd -= Math.sign(fwdVel) * brakeImpulse;
            }

            // Friction circle: clamp the combined tyre impulse to μ·N·dt.
            const budget = w.friction * susForce * dt;
            const mag = Math.sqrt(jFwd * jFwd + jSide * jSide);
            if (mag > budget && mag > 0) {
                const scale = budget / mag;
                jFwd *= scale; jSide *= scale;
            }

            if (jFwd !== 0 || jSide !== 0) {
                this.__force.x = fwx * jFwd + sx * jSide;
                this.__force.y = fwy * jFwd + sy * jSide;
                this.__force.z = fwz * jFwd + sz * jSide;
                this.__point.x = cx; this.__point.y = cy; this.__point.z = cz;
                this.system.applyImpulseAt(rb, tr, this.__force, this.__point);
            }

            // Visual wheel spin from forward ground speed.
            if (w.radius > 0) w.rotation += (fwdVel / w.radius) * dt;
        }
    }
}
