import { computeHashFloat } from "../../../core/primitives/numbers/computeHashFloat.js";
import Vector3 from "../../../core/geom/Vector3.js";
import Quaternion from "../../../core/geom/Quaternion.js";
import { DofMode } from "../constraint/DofMode.js";

/**
 * Sentinel for {@link Joint#_jointId} before the joint is linked into a
 * {@link PhysicsSystem}.
 * @type {number}
 */
export const JOINT_UNALLOCATED = -1;

/**
 * `entityB` value meaning "the world" — body A is constrained to a fixed
 * world anchor rather than to a second body.
 * @type {number}
 */
export const JOINT_WORLD = -1;

/**
 * A configurable 6-DOF constraint between two bodies (or a body and the
 * world). Connects body A's anchor frame to body B's; each of the 6 relative
 * degrees of freedom (3 linear, 3 angular) is independently
 * locked / free / limited / springy / motorised ({@link DofMode}). One
 * constraint type expresses the whole joint taxonomy — see DofMode.
 *
 * The default configuration is a **ball-socket** (point-to-point): the three
 * linear DOFs LOCKED (the two anchor points are held coincident), the three
 * angular DOFs FREE. That alone gives chains, ropes, and pendulums.
 *
 * Anchors live in body-local frames. When `entityB === JOINT_WORLD`,
 * `localAnchorB` is interpreted as a fixed **world** point instead of a
 * local anchor on a second body.
 *
 * Solver state ({@link dofImpulse}, the `_*` fields) is owned by the
 * PhysicsSystem; user code must not write it.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */
export class Joint {

    /**
     * Entity owning body A (the constraint's first body — always a real body).
     * @type {number}
     */
    entityA = -1;

    /**
     * Entity owning body B, or {@link JOINT_WORLD} to anchor A to the world.
     * @type {number}
     */
    entityB = JOINT_WORLD;

    /**
     * Anchor point on body A, in A's local frame.
     * @readonly
     * @type {Vector3}
     */
    localAnchorA = new Vector3(0, 0, 0);

    /**
     * Anchor point on body B in B's local frame — or, when
     * `entityB === JOINT_WORLD`, a fixed world-space anchor point.
     * @readonly
     * @type {Vector3}
     */
    localAnchorB = new Vector3(0, 0, 0);

    /**
     * Orientation of the joint's reference frame on body A, in A's local
     * space (a unit quaternion; identity = the joint frame is the body
     * frame). The 3 linear and 3 angular DOFs are expressed in this frame:
     * locked/limited/etc. axes are the frame's axes, not world axes. For a
     * hinge, the free angular axis is one of these frame axes — orient the
     * frame so that axis is the hinge axis.
     * @readonly
     * @type {Quaternion}
     */
    localBasisA = new Quaternion();

    /**
     * Orientation of the joint's reference frame on body B, in B's local
     * space. At rest a fully-locked joint holds frame B aligned to frame A.
     * When `entityB === JOINT_WORLD`, the world frame is treated as identity
     * (angular locks hold A's frame to world axes).
     * @readonly
     * @type {Quaternion}
     */
    localBasisB = new Quaternion();

    /**
     * Per-DOF mode, 6 entries: `[lin x, lin y, lin z, ang x, ang y, ang z]`.
     * Default = ball-socket (linear locked, angular free).
     * @readonly
     * @type {Uint8Array}
     */
    dofMode = Uint8Array.from([
        DofMode.LOCKED, DofMode.LOCKED, DofMode.LOCKED,
        DofMode.FREE, DofMode.FREE, DofMode.FREE,
    ]);

    /**
     * How the three angular DOF positions are measured. `false` (default) uses
     * the per-axis small-angle rotation vector — exact at the rest pose and
     * cheap, correct for welds, hinges, and modest angular limits. `true`
     * switches to the **swing-twist** decomposition: angular X is the *twist*
     * (rotation about the bone / X axis) and angular Y/Z are the *swing* (tilt
     * off it), each measured as an exact angle. Use it for wide cone-twist
     * range-of-motion (ragdoll shoulders) where the small-angle measure
     * under-reports large angles. See {@link asConeTwist}.
     * @type {boolean}
     */
    swingTwist = false;

    /**
     * Lower / upper bounds per DOF, used by {@link DofMode.LIMITED}. Linear in
     * metres, angular in radians. (Reserved — consumed once the LIMITED mode
     * lands.)
     * @readonly
     * @type {Float64Array}
     */
    dofLowerLimit = new Float64Array(6);
    /** @readonly @type {Float64Array} */
    dofUpperLimit = new Float64Array(6);

    /**
     * Spring stiffness / damping per DOF, used by {@link DofMode.SPRING}.
     * (Reserved — consumed once the SPRING mode lands.)
     * @readonly
     * @type {Float64Array}
     */
    dofStiffness = new Float64Array(6);
    /** @readonly @type {Float64Array} */
    dofDamping = new Float64Array(6);

    /**
     * Motor target velocity / max force per DOF, used by
     * {@link DofMode.MOTOR}. (Reserved — consumed once the MOTOR mode lands.)
     * @readonly
     * @type {Float64Array}
     */
    dofMotorTargetVelocity = new Float64Array(6);
    /** @readonly @type {Float64Array} */
    dofMotorMaxForce = new Float64Array(6);

    /**
     * Accumulated constraint impulse per DOF — warm-start state carried across
     * frames. System-owned.
     * @readonly
     * @type {Float64Array}
     */
    dofImpulse = new Float64Array(6);

    /**
     * Resolved packed body id of A, set at link time. System-private.
     * @type {number}
     */
    _bodyIdA = -1;
    /**
     * Resolved packed body id of B, or {@link JOINT_WORLD}. System-private.
     * @type {number}
     */
    _bodyIdB = JOINT_WORLD;
    /**
     * Packed joint handle assigned at link. System-private.
     * @type {number}
     */
    _jointId = JOINT_UNALLOCATED;

    /**
     * Configure this joint as a ball-socket (point-to-point) between two
     * anchors. Linear DOFs locked, angular free — the default, provided as a
     * named helper for clarity at call sites.
     * @returns {Joint} this
     */
    asBallSocket() {
        this.dofMode[0] = DofMode.LOCKED;
        this.dofMode[1] = DofMode.LOCKED;
        this.dofMode[2] = DofMode.LOCKED;
        this.dofMode[3] = DofMode.FREE;
        this.dofMode[4] = DofMode.FREE;
        this.dofMode[5] = DofMode.FREE;
        return this;
    }

    /**
     * Configure as a weld (fixed) joint: all 6 DOFs locked, holding the two
     * bodies rigidly in their relative pose.
     * @returns {Joint} this
     */
    asWeld() {
        for (let i = 0; i < 6; i++) this.dofMode[i] = DofMode.LOCKED;
        return this;
    }

    /**
     * Configure as a hinge (revolute): 3 linear + 2 angular locked, free to
     * rotate about a single frame axis. Orient {@link localBasisA} /
     * {@link localBasisB} so that frame axis is the desired hinge axis.
     *
     * @param {number} [freeAngularAxis] which frame axis is free: 0 = x
     *   (default), 1 = y, 2 = z.
     * @returns {Joint} this
     */
    asHinge(freeAngularAxis = 0) {
        this.dofMode[0] = DofMode.LOCKED;
        this.dofMode[1] = DofMode.LOCKED;
        this.dofMode[2] = DofMode.LOCKED;
        this.dofMode[3] = DofMode.LOCKED;
        this.dofMode[4] = DofMode.LOCKED;
        this.dofMode[5] = DofMode.LOCKED;
        this.dofMode[3 + freeAngularAxis] = DofMode.FREE;
        return this;
    }

    /**
     * Configure as a prismatic (slider): 2 linear + 3 angular locked, free to
     * translate along a single frame axis. Orient {@link localBasisA} /
     * {@link localBasisB} so that frame axis is the desired slide direction.
     *
     * @param {number} [freeLinearAxis] which frame axis is free: 0 = x
     *   (default), 1 = y, 2 = z.
     * @returns {Joint} this
     */
    asPrismatic(freeLinearAxis = 0) {
        this.dofMode[0] = DofMode.LOCKED;
        this.dofMode[1] = DofMode.LOCKED;
        this.dofMode[2] = DofMode.LOCKED;
        this.dofMode[freeLinearAxis] = DofMode.FREE;
        this.dofMode[3] = DofMode.LOCKED;
        this.dofMode[4] = DofMode.LOCKED;
        this.dofMode[5] = DofMode.LOCKED;
        return this;
    }

    /**
     * Configure as a cone-twist (ragdoll) joint: a ball-socket point (3 linear
     * locked) whose angular motion is a limited **twist** about the bone (frame
     * X) and a limited **swing** cone off it (frame Y / Z). Enables
     * {@link swingTwist} so the limits are measured as exact angles, holding
     * accurately at the wide ranges ragdoll shoulders need.
     *
     * Orient {@link localBasisA} / {@link localBasisB} so frame X runs along the
     * bone. The swing cone is box-shaped: independent Y and Z half-angles
     * (`swingLimitZ` defaults to `swingLimitY` for a circular cone).
     *
     * @param {number} twistLower @param {number} twistUpper  twist bounds, rad.
     * @param {number} swingLimitY  swing half-angle about frame Y, rad.
     * @param {number} [swingLimitZ]  swing half-angle about frame Z, rad
     *   (defaults to `swingLimitY`).
     * @returns {Joint} this
     */
    asConeTwist(twistLower, twistUpper, swingLimitY, swingLimitZ = swingLimitY) {
        this.dofMode[0] = DofMode.LOCKED;
        this.dofMode[1] = DofMode.LOCKED;
        this.dofMode[2] = DofMode.LOCKED;
        this.swingTwist = true;
        this.dofMode[3] = DofMode.LIMITED;
        this.dofLowerLimit[3] = twistLower;
        this.dofUpperLimit[3] = twistUpper;
        this.dofMode[4] = DofMode.LIMITED;
        this.dofLowerLimit[4] = -swingLimitY;
        this.dofUpperLimit[4] = swingLimitY;
        this.dofMode[5] = DofMode.LIMITED;
        this.dofLowerLimit[5] = -swingLimitZ;
        this.dofUpperLimit[5] = swingLimitZ;
        return this;
    }

    /**
     * Limit a linear DOF to a travel range along its frame axis (slider
     * end-stops): the DOF slides freely within `[lower, upper]` metres and is
     * resisted at the stops. Sets {@link DofMode.LIMITED} on that axis.
     *
     * @param {number} axis  frame axis: 0 = x, 1 = y, 2 = z.
     * @param {number} lower @param {number} upper  travel bounds in metres
     *   (signed anchor-A offset from anchor-B along the frame axis).
     * @returns {Joint} this
     */
    setLinearLimit(axis, lower, upper) {
        this.dofMode[axis] = DofMode.LIMITED;
        this.dofLowerLimit[axis] = lower;
        this.dofUpperLimit[axis] = upper;
        return this;
    }

    /**
     * Limit an angular DOF to a rotation range about its frame axis (joint
     * range-of-motion): free within `[lower, upper]` radians, resisted at the
     * stops. Sets {@link DofMode.LIMITED} on that angular axis.
     *
     * The angular position is a first-order (small-angle) measure, so the
     * effective stop is accurate for modest ranges (hinge / slider end-stops)
     * but under-reports large angles — wide cones (ragdoll shoulders) want the
     * swing-twist decomposition (already available as
     * `Quaternion.computeSwingAndTwist` / `computeTwistAngle`). See the solver
     * module docs.
     *
     * @param {number} axis  frame axis: 0 = x, 1 = y, 2 = z.
     * @param {number} lower @param {number} upper  rotation bounds in radians.
     * @returns {Joint} this
     */
    setAngularLimit(axis, lower, upper) {
        this.dofMode[3 + axis] = DofMode.LIMITED;
        this.dofLowerLimit[3 + axis] = lower;
        this.dofUpperLimit[3 + axis] = upper;
        return this;
    }

    /**
     * Drive a linear DOF at a target speed along its frame axis (piston,
     * conveyor, powered slider). A force-limited velocity servo: it pulls the
     * relative velocity toward `targetVelocity` as hard as `maxForce` allows.
     * Sets {@link DofMode.MOTOR} on that axis.
     *
     * @param {number} axis  frame axis: 0 = x, 1 = y, 2 = z.
     * @param {number} targetVelocity  desired relative anchor speed of A w.r.t.
     *   B along the frame axis, m/s (sign convention A − B).
     * @param {number} maxForce  force budget in newtons (`Infinity` for an
     *   ideal velocity drive; `0` makes the motor inert).
     * @returns {Joint} this
     */
    setLinearMotor(axis, targetVelocity, maxForce) {
        this.dofMode[axis] = DofMode.MOTOR;
        this.dofMotorTargetVelocity[axis] = targetVelocity;
        this.dofMotorMaxForce[axis] = maxForce;
        return this;
    }

    /**
     * Drive an angular DOF at a target rate about its frame axis (wheel drive,
     * powered hinge / door, turntable). A force-(torque-)limited velocity servo
     * pulling the relative angular velocity toward `targetVelocity`. Sets
     * {@link DofMode.MOTOR} on that angular axis.
     *
     * @param {number} axis  frame axis: 0 = x, 1 = y, 2 = z.
     * @param {number} targetVelocity  desired relative angular rate of B w.r.t.
     *   A about the frame axis, rad/s (sign convention B − A — so for a
     *   world-anchored motor a positive target spins body A negatively about
     *   the axis).
     * @param {number} maxForce  torque budget in newton-metres (`Infinity` for
     *   an ideal drive; `0` makes the motor inert).
     * @returns {Joint} this
     */
    setAngularMotor(axis, targetVelocity, maxForce) {
        this.dofMode[3 + axis] = DofMode.MOTOR;
        this.dofMotorTargetVelocity[3 + axis] = targetVelocity;
        this.dofMotorMaxForce[3 + axis] = maxForce;
        return this;
    }

    /**
     * Make a linear DOF a compliant spring along its frame axis (suspension,
     * bungee, soft-return slider). The spring drives the DOF toward its zero
     * (the rest pose — place the anchors / basis so the relaxed position is
     * `C·axis = 0`) with stiffness and damping, yielding under load rather than
     * holding rigid. Sets {@link DofMode.SPRING} on that axis.
     *
     * @param {number} axis  frame axis: 0 = x, 1 = y, 2 = z.
     * @param {number} stiffness  spring constant `k`, N/m (0 ⇒ pure damper).
     * @param {number} damping    damping coefficient `c`, N·s/m (0 ⇒ undamped).
     * @returns {Joint} this
     */
    setLinearSpring(axis, stiffness, damping) {
        this.dofMode[axis] = DofMode.SPRING;
        this.dofStiffness[axis] = stiffness;
        this.dofDamping[axis] = damping;
        return this;
    }

    /**
     * Make an angular DOF a compliant torsional spring about its frame axis
     * (soft-return hinge, sway bar, soft ragdoll joint). Drives the relative
     * rotation about the axis toward zero with stiffness and damping. Sets
     * {@link DofMode.SPRING} on that angular axis. Uses the small-angle measure
     * (see the solver module docs), so it is for modest ranges.
     *
     * @param {number} axis  frame axis: 0 = x, 1 = y, 2 = z.
     * @param {number} stiffness  torsional spring constant `k`, N·m/rad
     *   (0 ⇒ pure damper).
     * @param {number} damping    torsional damping, N·m·s/rad (0 ⇒ undamped).
     * @returns {Joint} this
     */
    setAngularSpring(axis, stiffness, damping) {
        this.dofMode[3 + axis] = DofMode.SPRING;
        this.dofStiffness[3 + axis] = stiffness;
        this.dofDamping[3 + axis] = damping;
        return this;
    }

    /**
     * Value equality over the configurable / persistent state — the same field
     * set that {@link JointSerializationAdapter} writes: the two anchor
     * entities, the local anchor points and basis frames, the swing-twist flag,
     * and the six per-DOF mode + limit / spring / motor arrays. Transient
     * solver state (`dofImpulse`) and the system-resolved handles (`_bodyIdA`,
     * `_bodyIdB`, `_jointId`) are excluded.
     *
     * @param {Joint} other
     * @returns {boolean}
     */
    equals(other) {
        if (other === this) return true;
        if (other === null || other === undefined || other.isJoint !== true) return false;

        if (this.entityA !== other.entityA
            || this.entityB !== other.entityB
            || this.swingTwist !== other.swingTwist
            || !this.localAnchorA.equals(other.localAnchorA)
            || !this.localAnchorB.equals(other.localAnchorB)
            || !this.localBasisA.equals(other.localBasisA)
            || !this.localBasisB.equals(other.localBasisB)
        ) {
            return false;
        }

        for (let i = 0; i < 6; i++) {
            if (this.dofMode[i] !== other.dofMode[i]
                || this.dofLowerLimit[i] !== other.dofLowerLimit[i]
                || this.dofUpperLimit[i] !== other.dofUpperLimit[i]
                || this.dofStiffness[i] !== other.dofStiffness[i]
                || this.dofDamping[i] !== other.dofDamping[i]
                || this.dofMotorTargetVelocity[i] !== other.dofMotorTargetVelocity[i]
                || this.dofMotorMaxForce[i] !== other.dofMotorMaxForce[i]
            ) {
                return false;
            }
        }

        return true;
    }

    /**
     * Hash over the same persistent state as {@link equals}. Equal joints hash
     * equal.
     * @returns {number}
     */
    hash() {
        let h = this.entityA | 0;
        h = (h * 31 + (this.entityB | 0)) | 0;
        h = (h * 31 + (this.swingTwist ? 1 : 0)) | 0;
        h = (h * 31 + this.localAnchorA.hash()) | 0;
        h = (h * 31 + this.localAnchorB.hash()) | 0;
        h = (h * 31 + this.localBasisA.hash()) | 0;
        h = (h * 31 + this.localBasisB.hash()) | 0;

        for (let i = 0; i < 6; i++) {
            h = (h * 31 + (this.dofMode[i] | 0)) | 0;
            h = (h * 31 + computeHashFloat(this.dofLowerLimit[i])) | 0;
            h = (h * 31 + computeHashFloat(this.dofUpperLimit[i])) | 0;
            h = (h * 31 + computeHashFloat(this.dofStiffness[i])) | 0;
            h = (h * 31 + computeHashFloat(this.dofDamping[i])) | 0;
            h = (h * 31 + computeHashFloat(this.dofMotorTargetVelocity[i])) | 0;
            h = (h * 31 + computeHashFloat(this.dofMotorMaxForce[i])) | 0;
        }

        return h;
    }
}

/**
 * @readonly
 * @type {string}
 */
Joint.typeName = "Joint";

/**
 * @readonly
 * @type {boolean}
 */
Joint.prototype.isJoint = true;
