import { EPSILON } from './types.js';
import { buildConstraints, buildSurfaceFaces } from './constraints.js';
import { simulate, applyAngleLimits, applyResult } from './solver.js';
import { Vector3, Quaternion, Matrix4x4, clamp01 } from '@zephyr3d/base';

// High-level orchestrator - SPCRJointDynamicsController API
/**
 * High-level controller for joint dynamics simulation.
 *
 * This class manages the physics simulation for a hierarchy of bones, including constraints,
 * colliders, and grabbers. It provides an API for initializing the system with bone data and
 * transforms, stepping the simulation each frame, and modifying colliders and grabbers at runtime.
 * The controller handles the internal state and logic for blending between animation and physics,
 * applying forces, and enforcing constraints.
 *
 * @public
 */ class JointDynamicsSystemController {
    _config;
    _pointsR = [];
    _pointsRW = [];
    _constraints = [];
    _collidersR = [];
    _collidersRW = [];
    _grabbersR = [];
    _grabbersRW = [];
    _movableLimitTargets = [];
    _flatPlanes = [];
    _flatPlaneEnabled = [];
    _flatPlaneAll = [];
    _surfaceConstraints = [];
    _positionsToTransform = [];
    _fakeWaveCounter = 0;
    _previousRootPosition = Vector3.zero();
    _previousRootRotation = Quaternion.identity();
    _rootTransform = null;
    _pointTransforms = [];
    _colliderTransforms = [];
    _grabberTransforms = [];
    _colliderHandleIds = [];
    _flatPlaneHandleIds = [];
    _grabberHandleIds = [];
    _colliderHandleToIndex = new Map();
    _flatPlaneHandleToIndex = new Map();
    _grabberHandleToIndex = new Map();
    _nextColliderHandleId = 1;
    _nextFlatPlaneHandleId = 1;
    _nextGrabberHandleId = 1;
    _initialized = false;
    _isPaused = false;
    _fadeState = 'none';
    _fadeTimer = 0;
    _fadeDuration = 0;
    constructor(config){
        this._config = config;
    }
    /**
   * Initializes the physics system.
   * @param rootTransform - Root bone transform used to detect root motion.
   * @param rootPoints - Root nodes of the bone hierarchy. Multiple roots are allowed.
   * @param pointTransforms - Transform array for all physics points. Order must match `BoneNode.index`.
   * @param colliders - Collider array (spheres/capsules).
   * @param grabbers - Grabber array used for interactions such as mouse dragging.
   * @param flatPlanes - Plane limiters such as the floor to prevent penetration.
   */ initialize(rootTransform, rootPoints, pointTransforms, colliders, grabbers, flatPlanes) {
        this._rootTransform = rootTransform;
        this._pointTransforms = pointTransforms;
        this._colliderTransforms = colliders.map((c)=>c.transform);
        this._grabberTransforms = grabbers.map((g)=>g.transform);
        // Build constraints
        this._constraints = buildConstraints(rootPoints, this._config.constraintOptions);
        this._surfaceConstraints = buildSurfaceFaces(rootPoints, this._config.constraintOptions.isLoop);
        // Flatten bone hierarchy to point list, build parent map
        const allPoints = [];
        const parentMap = new Map(); // child index -> parent index
        function walk(node) {
            allPoints.push(node);
            for (const c of node.children){
                parentMap.set(c.index, node.index);
                walk(c);
            }
        }
        for (const r of rootPoints){
            walk(r);
        }
        const maxDepth = Math.max(...allPoints.map((p)=>p.depth));
        // Compute boneAxis from transforms: local-space direction to first child
        // This is critical for correct bone rotation in skinned meshes
        const indexToNode = new Map();
        for (const p of allPoints){
            indexToNode.set(p.index, p);
        }
        for (const p of allPoints){
            if (!p.boneAxis && p.children.length > 0) {
                const childPos = pointTransforms[p.children[0].index].getWorldPosition();
                const parentPos = pointTransforms[p.index].getWorldPosition();
                // InverseTransformPoint: convert child world pos to parent local space
                const parentRot = pointTransforms[p.index].getWorldRotation();
                const parentScale = pointTransforms[p.index].getLocalScale();
                const diff = Vector3.sub(childPos, parentPos);
                const invRot = Quaternion.inverse(parentRot);
                const localDir = invRot.transform(diff);
                // Apply inverse scale
                const unscaled = new Vector3(parentScale.x !== 0 ? localDir.x / parentScale.x : 0, parentScale.y !== 0 ? localDir.y / parentScale.y : 0, parentScale.z !== 0 ? localDir.z / parentScale.z : 0);
                p.boneAxis = Vector3.normalize(unscaled);
            }
        }
        // Build PointR/RW arrays
        this._pointsR = allPoints.map((p)=>this._createPointR(p, maxDepth, parentMap, pointTransforms));
        this._pointsRW = allPoints.map(()=>this._createPointRW());
        this._positionsToTransform = new Array(allPoints.length).fill(Vector3.zero());
        // Initialize colliders
        this._collidersR = colliders.map((c)=>c.r);
        this._collidersRW = colliders.map((c)=>this._createColliderRW(c.transform));
        this._colliderHandleIds = this._collidersR.map(()=>this._nextColliderHandleId++);
        this._rebuildColliderHandleMap();
        // Initialize grabbers
        this._grabbersR = grabbers.map((g)=>g.r);
        this._grabbersRW = grabbers.map((g)=>({
                enabled: g.enabled ? 1 : 0,
                position: g.transform.getWorldPosition()
            }));
        this._grabberHandleIds = this._grabbersR.map(()=>this._nextGrabberHandleId++);
        this._rebuildGrabberHandleMap();
        // Flat planes
        this._flatPlanes = flatPlanes.map((fp)=>({
                normal: Vector3.normalize(fp.up),
                distance: -Vector3.dot(Vector3.normalize(fp.up), fp.position)
            }));
        this._flatPlaneAll = [
            ...this._flatPlanes
        ];
        this._flatPlaneEnabled = this._flatPlaneAll.map(()=>true);
        this._flatPlaneHandleIds = this._flatPlaneAll.map(()=>this._nextFlatPlaneHandleId++);
        this._rebuildFlatPlaneHandleMap();
        // Capture initial state
        this._previousRootPosition = rootTransform.getWorldPosition();
        this._previousRootRotation = rootTransform.getWorldRotation();
        for(let i = 0; i < this._pointsRW.length; i++){
            const pos = pointTransforms[i].getWorldPosition();
            this._pointsRW[i].positionCurrent = pos.clone();
            this._pointsRW[i].positionPrevious = pos.clone();
            this._pointsRW[i].positionCurrentTransform = pos.clone();
            this._pointsRW[i].positionPreviousTransform = pos.clone();
            this._pointsRW[i].positionToTransform = pos.clone();
            this._pointsRW[i].directionPrevious = Vector3.axisPZ();
            this._pointsRW[i].fakeWindDirection = Vector3.axisPZ();
            this._pointsRW[i].grabberIndex = -1;
            this._pointsRW[i].grabberDistance = 0;
            this._pointsRW[i].friction = 0;
        }
        this._initialized = true;
    }
    /**
   * Advances the simulation by one frame.
   * Internal flow: read current transforms, run Verlet integration and constraint solving,
   * then write the result back to the transforms.
   * @param deltaTime - Frame delta time in seconds. Internally subdivided by `subSteps`.
   */ step(deltaTime) {
        if (!this._initialized || !this._rootTransform) {
            return;
        }
        // Update fade
        if (this._fadeState !== 'none') {
            this._fadeTimer += deltaTime;
            if (this._fadeTimer >= this._fadeDuration) {
                this._fadeState = 'none';
                this._fadeTimer = 0;
            }
        }
        const blendRatio = this._computeBlendRatio();
        // Capture current transforms
        const rootPos = this._rootTransform.getWorldPosition();
        const rootRot = this._rootTransform.getWorldRotation();
        for(let i = 0; i < this._pointsRW.length; i++){
            this._pointsRW[i].positionPreviousTransform = this._pointsRW[i].positionCurrentTransform.clone();
            this._pointsRW[i].positionCurrentTransform = this._pointTransforms[i].getWorldPosition();
        }
        for(let i = 0; i < this._collidersRW.length; i++){
            const t = this._colliderTransforms[i];
            this._collidersRW[i].positionPreviousTransform = this._collidersRW[i].positionCurrentTransform.clone();
            this._collidersRW[i].directionPreviousTransform = this._collidersRW[i].directionCurrentTransform.clone();
            this._collidersRW[i].positionCurrentTransform = t.getWorldPosition();
            this._collidersRW[i].directionCurrentTransform = t.getWorldRotation();
            this._collidersRW[i].worldScale = t.getWorldScale();
            this._collidersRW[i].worldToLocal = Matrix4x4.compose(this._collidersRW[i].worldScale, this._collidersRW[i].directionCurrentTransform, this._collidersRW[i].positionCurrentTransform).inplaceInvertAffine();
        }
        for(let i = 0; i < this._grabbersRW.length; i++){
            this._grabbersRW[i].position = this._grabberTransforms[i].getWorldPosition();
        }
        // Run simulation
        const params = {
            isPaused: this._isPaused,
            stepTime: deltaTime,
            subSteps: this._config.subSteps,
            rootPosition: rootPos,
            previousRootPosition: this._previousRootPosition,
            rootSlideLimit: this._config.rootSlideLimit,
            rootRotation: rootRot,
            previousRootRotation: this._previousRootRotation,
            rootRotateLimit: this._config.rootRotateLimit,
            windForce: this._config.windForce,
            enableSurfaceCollision: this._config.enableSurfaceCollision,
            surfaceConstraints: this._surfaceConstraints,
            relaxation: this._config.relaxation,
            constraintShrinkLimit: this._config.constraintShrinkLimit,
            blendRatio,
            isFakeWave: this._config.isFakeWave,
            fakeWaveSpeed: this._config.fakeWaveSpeed,
            fakeWavePower: this._config.fakeWavePower,
            fakeWaveCounter: this._fakeWaveCounter,
            collisionScale: 1.0,
            enableBroadPhase: this._config.enableBroadPhase
        };
        const result = simulate(params, this._pointsR, this._pointsRW, this._constraints, this._collidersR, this._collidersRW, this._grabbersR, this._grabbersRW, this._movableLimitTargets, this._flatPlanes);
        this._positionsToTransform = result.positionsToTransform;
        this._fakeWaveCounter = result.fakeWaveCounter;
        // Angle limits
        if (this._config.angleLimitConfig.angleLimit >= 0) {
            applyAngleLimits(this._pointsR, this._pointsRW, this._config.angleLimitConfig);
        }
        // Apply results
        const transformRots = this._pointTransforms.map((t)=>t.getWorldRotation());
        const transformLocalRots = this._pointTransforms.map((t)=>t.getLocalRotation());
        const outputs = applyResult(this._pointsR, this._pointsRW, this._positionsToTransform, blendRatio, transformRots, transformLocalRots, this._config.preserveTwist);
        for(let i = 0; i < outputs.length; i++){
            this._pointTransforms[i].setWorldPosition(outputs[i].position);
            this._pointTransforms[i].setWorldRotation(outputs[i].rotation);
        }
        this._previousRootPosition = rootPos;
        this._previousRootRotation = rootRot;
    }
    /**
   * Returns the simulated results for all points as world-space positions and rotations.
   * Usually this does not need to be called manually because `step()` already writes the
   * output back to the transforms.
   */ getResults() {
        const transformRots = this._pointTransforms.map((t)=>t.getWorldRotation());
        const transformLocalRots = this._pointTransforms.map((t)=>t.getLocalRotation());
        const outputs = applyResult(this._pointsR, this._pointsRW, this._positionsToTransform, this._config.blendRatio, transformRots, transformLocalRots, this._config.preserveTwist);
        return outputs.map((o)=>({
                position: o.position,
                rotation: o.rotation
            }));
    }
    /**
   * Compensates for teleportation by resetting the previous root transform to the current one.
   * Call this after a character warp or teleport to avoid a large root-motion impulse on the
   * next simulation step.
   */ warp() {
        if (!this._rootTransform) {
            return;
        }
        this._previousRootPosition = this._rootTransform.getWorldPosition();
        this._previousRootRotation = this._rootTransform.getWorldRotation();
    }
    /**
   * Resets all physics state.
   * Each simulated point is snapped back to the current transform position and grab state
   * is cleared.
   */ reset() {
        for(let i = 0; i < this._pointsRW.length; i++){
            const pos = this._pointTransforms[i].getWorldPosition();
            this._pointsRW[i].positionCurrent = pos.clone();
            this._pointsRW[i].positionPrevious = pos.clone();
            this._pointsRW[i].grabberIndex = -1;
        }
        this._fakeWaveCounter = 0;
    }
    /**
   * Releases a fixed point so it becomes dynamic, for example when cloth is detached.
   * The point state is reset to avoid a sudden impulse.
   * @param index - Point index.
   */ releasePoint(index) {
        if (index < 0 || index >= this._pointsR.length) {
            return;
        }
        this._pointsR[index].weight = 1;
        const pos = this._pointTransforms[index].getWorldPosition();
        this._pointsRW[index].positionCurrent = pos.clone();
        this._pointsRW[index].positionPrevious = pos.clone();
        this._pointsRW[index].grabberIndex = -1;
    }
    /**
   * Fixes a dynamic point back to the animation pose, for example when an item is reattached.
   * The point grab state is cleared.
   * @param index - Point index.
   */ fixPoint(index) {
        if (index < 0 || index >= this._pointsR.length) {
            return;
        }
        this._pointsR[index].weight = 0;
        this._pointsRW[index].grabberIndex = -1;
    }
    /**
   * Returns whether a point is fixed to animation (`weight = 0`).
   * @param index - Point index.
   */ isPointFixed(index) {
        if (index < 0 || index >= this._pointsR.length) {
            return false;
        }
        return this._pointsR[index].weight <= EPSILON;
    }
    /** Gets the total number of physics points. */ get pointCount() {
        return this._pointsR.length;
    }
    /**
   * Fades physics in by blending from animation pose to simulation.
   * `blendRatio` moves from `1` (animation only) to `0` (physics only).
   * @param seconds - Transition duration in seconds.
   */ fadeIn(seconds) {
        this._fadeState = 'in';
        this._fadeTimer = 0;
        this._fadeDuration = seconds;
    }
    /**
   * Fades physics out by blending from simulation back to animation pose.
   * `blendRatio` moves from `0` (physics only) to `1` (animation only).
   * @param seconds - Transition duration in seconds.
   */ fadeOut(seconds) {
        this._fadeState = 'out';
        this._fadeTimer = 0;
        this._fadeDuration = seconds;
    }
    /**
   * Sets the global wind force vector.
   * The final wind contribution is still scaled per point by `windForceScale` and mass.
   * @param wind - Wind vector in world space.
   */ setWindForce(wind) {
        this._config.windForce = wind;
    }
    /**
   * Enable/disable broad-phase pruning for runtime performance comparison
   * @param enabled - `true` to enable broad-phase, `false` to disable.
   */ setBroadPhaseEnabled(enabled) {
        this._config.enableBroadPhase = enabled;
    }
    /** The blend ratio for the physics simulation */ get blendRatio() {
        return this._config.blendRatio;
    }
    set blendRatio(value) {
        this._config.blendRatio = clamp01(value);
    }
    /**
   * Pauses or resumes the physics simulation.
   * While paused, the system still follows root motion but skips force integration and
   * constraint solving.
   * @param paused - `true` to pause, `false` to resume.
   */ setPaused(paused) {
        this._isPaused = paused;
    }
    /**
   * Enable or disable a collider by current array index.
   * Transform state is still read from the collider's TransformAccess each frame.
   * Prefer setColliderEnabled(handle, enabled) for runtime-owned colliders.
   *
   * @param index - Collider index in the current array. Note that this may change when colliders are added or removed.
   * @param enabled - `true` to enable the collider, `false` to disable it.
   *
   * @returns `true` if the index is valid and the collider was updated, `false` if the index is out of range.
   */ setColliderEnabledAt(index, enabled) {
        if (index >= 0 && index < this._collidersRW.length) {
            this._collidersRW[index].enabled = enabled ? 1 : 0;
            return true;
        }
        return false;
    }
    /**
   * Enable or disable a runtime collider by stable handle.
   *
   * @param handle - Stable handle for the collider to enable or disable.
   * @param enabled - `true` to enable the collider, `false` to disable it.
   * @returns true if the handle is still valid.
   */ setColliderEnabled(handle, enabled) {
        const index = this._getColliderIndex(handle);
        if (index === -1) {
            return false;
        }
        return this.setColliderEnabledAt(index, enabled);
    }
    /**
   * Add a collider at runtime.
   * @param r - Read-only collider data such as shape and size.
   * @param transform - TransformAccess that provides the collider's position and rotation each frame.
   * @returns A stable handle that remains valid until this collider is removed.
   */ addCollider(r, transform) {
        const id = this._nextColliderHandleId++;
        this._collidersR.push(r);
        this._colliderTransforms.push(transform);
        this._collidersRW.push(this._createColliderRW(transform));
        this._colliderHandleIds.push(id);
        this._colliderHandleToIndex.set(id, this._collidersR.length - 1);
        return {
            type: 'collider',
            id
        };
    }
    /**
   * Remove a collider by stable handle.
   * @param handle - Stable handle for the collider to remove.
   * @returns true if the collider existed and was removed.
   */ removeCollider(handle) {
        const index = this._getColliderIndex(handle);
        if (index === -1) {
            return false;
        }
        return this.removeColliderAt(index);
    }
    /**
   * Remove a collider by current array index.
   * @param index - Collider index in the current array. Note that this may change when colliders are added or removed.
   * Prefer removeCollider(handle) for runtime-owned colliders.
   */ removeColliderAt(index) {
        if (index < 0 || index >= this._collidersR.length) {
            return false;
        }
        this._collidersR.splice(index, 1);
        this._colliderTransforms.splice(index, 1);
        this._collidersRW.splice(index, 1);
        this._colliderHandleIds.splice(index, 1);
        this._rebuildColliderHandleMap();
        return true;
    }
    /**
   * Enable or disable a flat plane by current array index.
   * @param index - Flat plane index in the current array. Note that this may change when flat planes are added or removed.
   * @param enabled - `true` to enable the flat plane, `false` to disable it.
   * @returns `true` if the index is valid and the flat plane was updated, `false` if the index is out of range.
   */ setFlatPlaneEnabledAt(index, enabled) {
        if (index < 0 || index >= this._flatPlaneAll.length) {
            return false;
        }
        this._flatPlaneEnabled[index] = enabled;
        this._rebuildActiveFlatPlanes();
        return true;
    }
    /**
   * Enable or disable a runtime flat plane by stable handle.
   * @param handle - Stable handle for the flat plane to enable or disable.
   * @param enabled - `true` to enable the flat plane, `false` to disable it.
   * @returns true if the handle is still valid.
   */ setFlatPlaneEnabled(handle, enabled) {
        const index = this._getFlatPlaneIndex(handle);
        if (index === -1) {
            return false;
        }
        return this.setFlatPlaneEnabledAt(index, enabled);
    }
    /**
   * Add a flat plane at runtime.
   * @param up - Up direction of the plane. The normal is computed as `Vector3.normalize(up)`.
   * @param position - A point on the plane. The distance is computed as `-Vector3.dot(normal, position)`.
   * @returns A stable handle that remains valid until this flat plane is removed.
   */ addFlatPlane(up, position) {
        const id = this._nextFlatPlaneHandleId++;
        const normal = Vector3.normalize(up);
        this._flatPlaneAll.push({
            normal,
            distance: -Vector3.dot(normal, position)
        });
        this._flatPlaneEnabled.push(true);
        this._flatPlaneHandleIds.push(id);
        this._flatPlaneHandleToIndex.set(id, this._flatPlaneAll.length - 1);
        this._rebuildActiveFlatPlanes();
        return {
            type: 'flatPlane',
            id
        };
    }
    /**
   * Remove a flat plane by stable handle.
   * @param handle - Stable handle for the flat plane to remove.
   * @returns true if the flat plane existed and was removed.
   */ removeFlatPlane(handle) {
        const index = this._getFlatPlaneIndex(handle);
        if (index === -1) {
            return false;
        }
        return this.removeFlatPlaneAt(index);
    }
    /**
   * Remove a flat plane by current array index.
   * @param index - Flat plane index in the current array. Note that this may change when flat planes are added or removed.
   * @returns true if the index was valid and the flat plane was removed, false if the index was out of range.
   */ removeFlatPlaneAt(index) {
        if (index < 0 || index >= this._flatPlaneAll.length) {
            return false;
        }
        this._flatPlaneAll.splice(index, 1);
        this._flatPlaneEnabled.splice(index, 1);
        this._flatPlaneHandleIds.splice(index, 1);
        this._rebuildFlatPlaneHandleMap();
        this._rebuildActiveFlatPlanes();
        return true;
    }
    /**
   * Enable or disable a grabber by current array index.
   * Transform state is still read from the grabber's TransformAccess each frame.
   * Prefer setGrabberEnabled(handle, enabled) for runtime-owned grabbers.
   *
   * @param index - Grabber index in the current array. Note that this may change when grabbers are added or removed.
   * @param enabled - `true` to enable the grabber, `false` to disable it.
   * @returns `true` if the index is valid and the grabber was updated, `false` if the index is out of range.
   */ setGrabberEnabledAt(index, enabled) {
        if (index >= 0 && index < this._grabbersRW.length) {
            this._grabbersRW[index].enabled = enabled ? 1 : 0;
            if (!enabled) {
                this._releaseGrabber(index);
            }
            return true;
        }
        return false;
    }
    /**
   * Enable or disable a runtime grabber by stable handle.
   * @param handle - Stable handle for the grabber to enable or disable.
   * @param enabled - `true` to enable the grabber, `false` to disable it.
   * @returns true if the handle is still valid.
   */ setGrabberEnabled(handle, enabled) {
        const index = this._getGrabberIndex(handle);
        if (index === -1) {
            return false;
        }
        return this.setGrabberEnabledAt(index, enabled);
    }
    /**
   * Add a grabber at runtime.
   * @param r - Read-only grabber data such as interaction radius.
   * @param transform - TransformAccess that provides the grabber's position each frame.
   * @param enabled - Whether the grabber starts enabled. The grabber can still be moved by its transform while disabled, but it won't affect any points until enabled.
   * @returns A stable handle that remains valid until this grabber is removed.
   */ addGrabber(r, transform, enabled = false) {
        const id = this._nextGrabberHandleId++;
        this._grabbersR.push(r);
        this._grabberTransforms.push(transform);
        this._grabbersRW.push({
            enabled: enabled ? 1 : 0,
            position: transform.getWorldPosition()
        });
        this._grabberHandleIds.push(id);
        this._grabberHandleToIndex.set(id, this._grabbersR.length - 1);
        return {
            type: 'grabber',
            id
        };
    }
    /**
   * Remove a grabber by stable handle.
   * @param handle - Stable handle for the grabber to remove.
   * @returns true if the grabber existed and was removed.
   */ removeGrabber(handle) {
        const index = this._getGrabberIndex(handle);
        if (index === -1) {
            return false;
        }
        return this.removeGrabberAt(index);
    }
    /**
   * Remove a grabber by current array index.
   * Prefer removeGrabber(handle) for runtime-owned grabbers.
   *
   * @param index - Grabber index in the current array. Note that this may change when grabbers are added or removed.
   * @returns true if the index was valid and the grabber was removed, false if the index was out of range.
   */ removeGrabberAt(index) {
        if (index < 0 || index >= this._grabbersR.length) {
            return false;
        }
        this._grabbersR.splice(index, 1);
        this._grabberTransforms.splice(index, 1);
        this._grabbersRW.splice(index, 1);
        this._grabberHandleIds.splice(index, 1);
        this._rebuildGrabberHandleMap();
        this._releaseGrabber(index);
        for (const ptRW of this._pointsRW){
            if (ptRW.grabberIndex > index) {
                ptRW.grabberIndex--;
            }
        }
        return true;
    }
    /** Gets the number of runtime colliders. */ get colliderCount() {
        return this._collidersR.length;
    }
    /** Gets the number of runtime flat planes. */ get flatPlaneCount() {
        return this._flatPlaneAll.length;
    }
    /** Gets the number of runtime grabbers. */ get grabberCount() {
        return this._grabbersR.length;
    }
    _computeBlendRatio() {
        if (this._fadeState === 'in') {
            return 1.0 - clamp01(this._fadeTimer / this._fadeDuration);
        }
        if (this._fadeState === 'out') {
            return clamp01(this._fadeTimer / this._fadeDuration);
        }
        return this._config.blendRatio;
    }
    _getColliderIndex(handle) {
        if (handle.type !== 'collider') {
            return -1;
        }
        return this._colliderHandleToIndex.get(handle.id) ?? -1;
    }
    _getGrabberIndex(handle) {
        if (handle.type !== 'grabber') {
            return -1;
        }
        return this._grabberHandleToIndex.get(handle.id) ?? -1;
    }
    _getFlatPlaneIndex(handle) {
        if (handle.type !== 'flatPlane') {
            return -1;
        }
        return this._flatPlaneHandleToIndex.get(handle.id) ?? -1;
    }
    _releaseGrabber(index) {
        for (const ptRW of this._pointsRW){
            if (ptRW.grabberIndex === index) {
                ptRW.grabberIndex = -1;
                ptRW.grabberDistance = 0;
            }
        }
    }
    _rebuildColliderHandleMap() {
        this._colliderHandleToIndex.clear();
        for(let i = 0; i < this._colliderHandleIds.length; i++){
            this._colliderHandleToIndex.set(this._colliderHandleIds[i], i);
        }
    }
    _rebuildFlatPlaneHandleMap() {
        this._flatPlaneHandleToIndex.clear();
        for(let i = 0; i < this._flatPlaneHandleIds.length; i++){
            this._flatPlaneHandleToIndex.set(this._flatPlaneHandleIds[i], i);
        }
    }
    _rebuildActiveFlatPlanes() {
        this._flatPlanes = [];
        for(let i = 0; i < this._flatPlaneAll.length; i++){
            if (this._flatPlaneEnabled[i]) {
                this._flatPlanes.push(this._flatPlaneAll[i]);
            }
        }
    }
    _rebuildGrabberHandleMap() {
        this._grabberHandleToIndex.clear();
        for(let i = 0; i < this._grabberHandleIds.length; i++){
            this._grabberHandleToIndex.set(this._grabberHandleIds[i], i);
        }
    }
    _createPointR(node, maxDepth, parentMap, transforms) {
        const rate = maxDepth > 0 ? node.depth / maxDepth : 0;
        const c = this._config.curves;
        const parentIdx = parentMap.get(node.index) ?? -1;
        const childIdx = node.children.length > 0 ? node.children[0].index : -1;
        // Capture initial local transform for rotation blending
        const t = transforms[node.index];
        const initLocalPos = t.getLocalPosition();
        const initLocalRot = t.getLocalRotation();
        const initLocalScale = t.getLocalScale();
        // BoneAxis: local-space direction to first child (computed in initialize)
        const boneAxis = node.boneAxis ?? new Vector3(0, -1, 0);
        // ParentLength: distance to parent
        let parentLength = 0;
        if (parentIdx !== -1) {
            const pPos = transforms[parentIdx].getWorldPosition();
            const cPos = transforms[node.index].getWorldPosition();
            parentLength = Vector3.distance(pPos, cPos);
        }
        return {
            parent: parentIdx,
            child: childIdx,
            applyInvertCollision: 0,
            movableLimitIndex: -1,
            movableLimitRadius: 0,
            weight: node.isFixed ? 0 : 1,
            mass: c.massScale.evaluate(rate),
            resistance: clamp01(c.resistance.evaluate(rate)),
            hardness: clamp01(c.hardness.evaluate(rate)),
            frictionScale: c.friction.evaluate(rate),
            sliderJointLength: c.sliderJointLength.evaluate(rate),
            parentLength,
            structuralShrinkVertical: c.structuralShrinkVertical.evaluate(rate) * 0.5,
            structuralStretchVertical: c.structuralStretchVertical.evaluate(rate) * 0.5,
            structuralShrinkHorizontal: c.structuralShrinkHorizontal.evaluate(rate) * 0.5,
            structuralStretchHorizontal: c.structuralStretchHorizontal.evaluate(rate) * 0.5,
            shearShrink: c.shearShrink.evaluate(rate) * 0.5,
            shearStretch: c.shearStretch.evaluate(rate) * 0.5,
            bendingShrinkVertical: c.bendingShrinkVertical.evaluate(rate) * 0.5,
            bendingStretchVertical: c.bendingStretchVertical.evaluate(rate) * 0.5,
            bendingShrinkHorizontal: c.bendingShrinkHorizontal.evaluate(rate) * 0.5,
            bendingStretchHorizontal: c.bendingStretchHorizontal.evaluate(rate) * 0.5,
            windForceScale: c.windForceScale.evaluate(rate) * rate,
            fakeWavePower: c.fakeWavePower.evaluate(rate),
            fakeWaveFreq: c.fakeWaveFreq.evaluate(rate),
            forceFadeRatio: 0,
            pointRadius: Math.max(0, c.pointRadius.evaluate(rate)),
            gravity: Vector3.scale(this._config.gravity, c.gravityScale.evaluate(rate)),
            boneAxis,
            initialLocalScale: initLocalScale,
            initialLocalRotation: initLocalRot,
            initialLocalTwist: (()=>{
                const tw = new Quaternion();
                initLocalRot.decomposeSwingTwist(boneAxis, undefined, tw);
                return tw;
            })(),
            initialLocalPosition: initLocalPos
        };
    }
    _createPointRW() {
        return {
            positionToTransform: Vector3.zero(),
            positionCurrentTransform: Vector3.zero(),
            positionPreviousTransform: Vector3.zero(),
            positionCurrent: Vector3.zero(),
            positionPrevious: Vector3.zero(),
            directionPrevious: Vector3.axisPZ(),
            fakeWindDirection: Vector3.axisPZ(),
            grabberIndex: -1,
            grabberDistance: 0,
            friction: 0
        };
    }
    _createColliderRW(transform) {
        const pos = transform.getWorldPosition();
        const rot = transform.getWorldRotation();
        const scale = transform.getWorldScale();
        return {
            positionCurrent: pos.clone(),
            directionCurrent: Vector3.zero(),
            boundsCenter: pos.clone(),
            boundsRadius: 0,
            positionCurrentTransform: pos.clone(),
            positionPreviousTransform: pos.clone(),
            directionCurrentTransform: rot.clone(),
            directionPreviousTransform: rot.clone(),
            worldToLocal: Matrix4x4.identity(),
            worldScale: scale.clone(),
            localBoundsMin: Vector3.zero(),
            localBoundsMax: Vector3.zero(),
            radius: 0,
            enabled: 1
        };
    }
}

export { JointDynamicsSystemController };
//# sourceMappingURL=controller.js.map
