import "@babylonjs/core/Physics/joinedPhysicsEngineComponent";
import "@babylonjs/core/Physics/v1/physicsEngineComponent";
import { BoundingInfo } from "@babylonjs/core/Culling/boundingInfo";
import { Matrix, Quaternion, Vector3 } from "@babylonjs/core/Maths/math.vector";
import { AbstractMesh } from "@babylonjs/core/Meshes/abstractMesh";
import { PhysicsImpostor } from "@babylonjs/core/Physics/v1/physicsImpostor";
import { PmxObject } from "../../Loader/Parser/pmxObject";
import { Generic6DofSpringJoint } from "./mmdAmmoJSPlugin";
class MmdPhysicsMesh extends AbstractMesh {
    linkedBone;
    physicsMode;
    bodyOffsetMatrix;
    bodyOffsetInverseMatrix;
    _customBoundingInfo;
    constructor(name, scene, linkedBone, physicsMode, customBoundingInfo) {
        super(name, scene);
        this.linkedBone = linkedBone;
        this.physicsMode = physicsMode;
        this.bodyOffsetMatrix = Matrix.Identity();
        this.bodyOffsetInverseMatrix = Matrix.Identity();
        this._customBoundingInfo = customBoundingInfo;
    }
    static _WorldMatrix = new Matrix();
    computeBodyOffsetMatrix(parentWorldMatrixInverse) {
        const worldMatrix = Matrix.ComposeToRef(this.scaling, this.rotationQuaternion, this.position, MmdPhysicsMesh._WorldMatrix);
        worldMatrix.multiplyToRef(parentWorldMatrixInverse, this.bodyOffsetMatrix);
        this.bodyOffsetMatrix.invertToRef(this.bodyOffsetInverseMatrix);
    }
    getBoundingInfo() {
        return this._customBoundingInfo ?? super.getBoundingInfo();
    }
}
class MmdAmmoPhysicsImpostor extends PhysicsImpostor {
    linkedBone;
    constructor(mesh, type, options, linkedBone, scene) {
        super(mesh, type, options, scene);
        this.linkedBone = linkedBone;
    }
}
/**
 * MMD ammo physics model is container of the ammo.js physics resources of the MMD model
 */
export class MmdAmmoPhysicsModel {
    _mmdPhysics;
    _nodes;
    _impostors;
    _rootMesh;
    // eslint-disable-next-line @typescript-eslint/consistent-type-imports
    _ammoInstance;
    // eslint-disable-next-line @typescript-eslint/consistent-type-imports
    _tmpBtVector3;
    // eslint-disable-next-line @typescript-eslint/consistent-type-imports
    _tmpBtQuaternion;
    // eslint-disable-next-line @typescript-eslint/consistent-type-imports
    _tmpBtTransform;
    /**
     * Create a new MMD ammo.js physics model
     * @param mmdPhysics MMD ammo physics
     * @param nodes MMD physics transform nodes
     * @param impostors Physics impostors
     * @param rootMesh Root mesh of the MMD model
     * @param ammoInstance Ammo.js instance
     */
    constructor(mmdPhysics, nodes, impostors, rootMesh, ammoInstance) {
        this._mmdPhysics = mmdPhysics;
        this._nodes = nodes;
        this._impostors = impostors;
        this._rootMesh = rootMesh;
        this._ammoInstance = ammoInstance;
        this._tmpBtVector3 = new ammoInstance.btVector3();
        this._tmpBtQuaternion = new ammoInstance.btQuaternion();
        this._tmpBtTransform = new ammoInstance.btTransform();
    }
    /**
     * Dispose the physics resources
     */
    dispose() {
        const impostors = this._impostors;
        for (let i = 0; i < impostors.length; ++i) {
            const impostor = impostors[i];
            if (impostor === null)
                continue;
            impostor.dispose();
            impostor.object.physicsImpostor = null;
        }
        impostors.length = 0;
        const nodes = this._nodes;
        for (let i = 0; i < nodes.length; ++i) {
            nodes[i]?.dispose(false, true);
        }
        nodes.length = 0;
        this._ammoInstance.destroy(this._tmpBtVector3);
        this._ammoInstance.destroy(this._tmpBtQuaternion);
        this._ammoInstance.destroy(this._tmpBtTransform);
    }
    static _NodeWorldMatrix = new Matrix();
    static _ZeroVector = Vector3.Zero();
    static _Position = new Vector3();
    static _Rotation = new Quaternion();
    /**
     * Reset the rigid body positions and velocities
     */
    initialize() {
        const modelWorldMatrix = this._rootMesh.computeWorldMatrix();
        const position = MmdAmmoPhysicsModel._Position;
        const rotation = MmdAmmoPhysicsModel._Rotation;
        const btVector3 = this._tmpBtVector3;
        const btQuaternion = this._tmpBtQuaternion;
        const btTransform = this._tmpBtTransform;
        const mmdPhysics = this._mmdPhysics;
        const nodes = this._nodes;
        for (let i = 0; i < nodes.length; ++i) {
            const node = nodes[i];
            if (node === null)
                continue;
            const nodeWorldMatrix = node.linkedBone.getWorldMatrixToRef(MmdAmmoPhysicsModel._NodeWorldMatrix);
            node.bodyOffsetMatrix.multiplyToRef(nodeWorldMatrix, nodeWorldMatrix);
            if (node.physicsMode === PmxObject.RigidBody.PhysicsMode.FollowBone) {
                nodeWorldMatrix.decompose(node.scaling, node.rotationQuaternion, node.position);
            }
            else {
                nodeWorldMatrix.multiplyToRef(modelWorldMatrix, nodeWorldMatrix);
                nodeWorldMatrix.decompose(undefined, rotation, position);
                const impostor = node.physicsImpostor;
                mmdPhysics._makeKinematicOnce(impostor);
                btVector3.setValue(position.x, position.y, position.z);
                btQuaternion.setValue(rotation.x, rotation.y, rotation.z, rotation.w);
                btTransform.setOrigin(btVector3);
                btTransform.setRotation(btQuaternion);
                // eslint-disable-next-line @typescript-eslint/consistent-type-imports
                const body = impostor.physicsBody;
                body.getMotionState().setWorldTransform(btTransform);
            }
        }
    }
    /**
     * Set the rigid bodies transform to the bones transform
     */
    syncBodies() {
        const nodes = this._nodes;
        for (let i = 0; i < nodes.length; ++i) {
            const node = nodes[i];
            if (node === null)
                continue;
            switch (node.physicsMode) {
                case PmxObject.RigidBody.PhysicsMode.FollowBone:
                    {
                        const nodeWorldMatrix = node.linkedBone.getWorldMatrixToRef(MmdAmmoPhysicsModel._NodeWorldMatrix);
                        node.bodyOffsetMatrix.multiplyToRef(nodeWorldMatrix, nodeWorldMatrix);
                        nodeWorldMatrix.decompose(node.scaling, node.rotationQuaternion, node.position);
                    }
                    break;
                case PmxObject.RigidBody.PhysicsMode.Physics:
                case PmxObject.RigidBody.PhysicsMode.PhysicsWithBone:
                    break;
                default:
                    throw new Error(`Unknown physics mode: ${node.physicsMode}`);
            }
        }
    }
    static _BoneWorldPosition = new Vector3();
    /**
     * Set the bones transform to the rigid bodies transform
     */
    syncBones() {
        const nodes = this._nodes;
        for (let i = 0; i < nodes.length; ++i) {
            const node = nodes[i];
            if (node === null)
                continue;
            switch (node.physicsMode) {
                case PmxObject.RigidBody.PhysicsMode.FollowBone:
                    break;
                case PmxObject.RigidBody.PhysicsMode.Physics:
                    {
                        node.bodyOffsetInverseMatrix.multiplyToArray(Matrix.ComposeToRef(node.scaling, node.rotationQuaternion, node.position, MmdAmmoPhysicsModel._NodeWorldMatrix), node.linkedBone.worldMatrix, 0);
                    }
                    break;
                case PmxObject.RigidBody.PhysicsMode.PhysicsWithBone:
                    {
                        node.linkedBone.getWorldTranslationToRef(MmdAmmoPhysicsModel._BoneWorldPosition);
                        node.bodyOffsetInverseMatrix.multiplyToArray(Matrix.ComposeToRef(node.scaling, node.rotationQuaternion, MmdAmmoPhysicsModel._ZeroVector, MmdAmmoPhysicsModel._NodeWorldMatrix), node.linkedBone.worldMatrix, 0);
                        node.linkedBone.setWorldTranslationFromRef(MmdAmmoPhysicsModel._BoneWorldPosition);
                    }
                    break;
                default:
                    throw new Error(`Unknown physics mode: ${node.physicsMode}`);
            }
        }
    }
}
/**
 * Use the v1 physics engine to build the physics model of the MMD model
 *
 * If you do not want to use a physics engine, you can reduce the bundling size by not import this class
 */
export class MmdAmmoPhysics {
    _scene;
    _kinematicOnces = [];
    /**
     * Create a new MMD ammo.js physics engine
     *
     * Scene must have a physics engine enabled
     * @param scene The scene to build the physics model
     */
    constructor(scene) {
        this._scene = scene;
    }
    /**
     * Build the physics model of the MMD model
     * @param rootMesh Root mesh of the MMD model
     * @param bones MMD runtime bones
     * @param rigidBodies rigid bodies information
     * @param joints joints information
     * @param logger Logger
     * @returns MMD physics model
     * @throws If the ammo physics engine is not enabled
     */
    buildPhysics(rootMesh, bones, rigidBodies, joints, logger) {
        const scene = this._scene;
        const physicsPlugin = scene.getPhysicsEngine()?.getPhysicsPlugin();
        if (!physicsPlugin) {
            throw new Error("Physics engine is not enabled");
        }
        if (physicsPlugin.name !== "MmdAmmoJSPlugin") {
            throw new Error("Physics engine is not MMDAmmoJSPlugin");
        }
        let scalingFactor;
        rootMesh.computeWorldMatrix(true);
        const worldMatrix = rootMesh.getWorldMatrix();
        const worldRotation = new Quaternion();
        {
            const worldScale = new Vector3();
            worldMatrix.decompose(worldScale, worldRotation);
            if (Math.abs(worldScale.x - worldScale.y) < 0.0001 && Math.abs(worldScale.y - worldScale.z) < 0.0001) {
                if (Math.abs(worldScale.x - 1) < 0.0001 && Math.abs(worldScale.y - 1) < 0.0001 && Math.abs(worldScale.z - 1) < 0.0001) {
                    scalingFactor = 1;
                }
                else {
                    scalingFactor = worldScale.x;
                    logger.warn("Root node scaling is not 1, simulation may differ from the original");
                }
            }
            else {
                scalingFactor = Math.max(worldScale.x, worldScale.y, worldScale.z);
                logger.warn("Root node scaling is not uniform, physics may not work correctly");
            }
        }
        const nodes = new Array(rigidBodies.length);
        const impostors = new Array(rigidBodies.length);
        const boneNameMap = new Map();
        for (let i = 0; i < bones.length; ++i) {
            const bone = bones[i];
            boneNameMap.set(bone.name, bone);
        }
        const resolveRigidBodyBone = (rigidBody) => {
            if (rigidBody.boneIndex < 0 || bones.length <= rigidBody.boneIndex) {
                return boneNameMap.get(rigidBody.name);
            }
            return bones[rigidBody.boneIndex];
        };
        for (let i = 0; i < rigidBodies.length; ++i) {
            const rigidBody = rigidBodies[i];
            const bone = resolveRigidBodyBone(rigidBody);
            if (bone === undefined) {
                logger.warn(`Bone index out of range failed to create rigid body: ${rigidBody.name}`);
                nodes[i] = null;
                impostors[i] = null;
                continue;
            }
            let impostorType;
            const boundMin = new Vector3();
            const boundMax = new Vector3();
            let isZeroVolume = false;
            switch (rigidBody.shapeType) {
                case PmxObject.RigidBody.ShapeType.Sphere: {
                    impostorType = PhysicsImpostor.SphereImpostor;
                    const radius = rigidBody.shapeSize[0] * scalingFactor;
                    boundMin.copyFromFloats(-radius, -radius, -radius);
                    boundMax.copyFromFloats(radius, radius, radius);
                    if (radius === 0)
                        isZeroVolume = true;
                    break;
                }
                case PmxObject.RigidBody.ShapeType.Box: {
                    impostorType = PhysicsImpostor.BoxImpostor;
                    const shapeSize = rigidBody.shapeSize;
                    boundMin.copyFromFloats(-shapeSize[0] * scalingFactor, -shapeSize[1] * scalingFactor, -shapeSize[2] * scalingFactor);
                    boundMax.copyFromFloats(shapeSize[0] * scalingFactor, shapeSize[1] * scalingFactor, shapeSize[2] * scalingFactor);
                    if (shapeSize[0] === 0 || shapeSize[1] === 0 || shapeSize[2] === 0)
                        isZeroVolume = true;
                    break;
                }
                case PmxObject.RigidBody.ShapeType.Capsule: {
                    impostorType = PhysicsImpostor.CapsuleImpostor;
                    const shapeSize = rigidBody.shapeSize;
                    const x = shapeSize[0] * scalingFactor;
                    const y = shapeSize[1] / 2 * scalingFactor + x;
                    boundMin.copyFromFloats(-x, -y, -x);
                    boundMax.copyFromFloats(x, y, x);
                    if (shapeSize[0] === 0 || shapeSize[1] === 0)
                        isZeroVolume = true;
                    break;
                }
                default:
                    logger.warn(`Unknown rigid body shape type: ${rigidBody.shapeType}`);
                    nodes[i] = null;
                    impostors[i] = null;
                    continue;
            }
            const node = new MmdPhysicsMesh(rigidBody.name, scene, bone, rigidBody.physicsMode, new BoundingInfo(boundMin, boundMax));
            const shapePosition = rigidBody.shapePosition;
            node.position.copyFromFloats(shapePosition[0], shapePosition[1], shapePosition[2]);
            const shapeRotation = rigidBody.shapeRotation;
            node.rotationQuaternion = Quaternion.FromEulerAngles(shapeRotation[0], shapeRotation[1], shapeRotation[2]);
            // compute the body offset matrix in local space
            node.computeBodyOffsetMatrix(bone.linkedBone.getAbsoluteInverseBindMatrix());
            // then convert the body transform to world space
            Vector3.TransformCoordinatesToRef(node.position, worldMatrix, node.position);
            worldRotation.multiplyToRef(node.rotationQuaternion, node.rotationQuaternion);
            const mass = rigidBody.physicsMode === PmxObject.RigidBody.PhysicsMode.FollowBone
                ? 0
                : rigidBody.mass * scalingFactor;
            // if mass is 0, the object will be constructed as a kinematic object by babylon.js physics plugin
            const impostor = node.physicsImpostor = new MmdAmmoPhysicsImpostor(node, impostorType, {
                mass: mass,
                friction: rigidBody.friction,
                restitution: rigidBody.repulsion,
                ignoreParent: true,
                disableBidirectionalTransformation: rigidBody.physicsMode !== PmxObject.RigidBody.PhysicsMode.FollowBone,
                group: 1 << rigidBody.collisionGroup,
                mask: rigidBody.collisionMask
            }, bone, scene);
            impostor.setDeltaPosition(new Vector3(0, 0, 0));
            node.setParent(rootMesh);
            // eslint-disable-next-line @typescript-eslint/consistent-type-imports
            const body = impostor.physicsBody;
            if (rigidBody.collisionMask === 0 || isZeroVolume) {
                body.setCollisionFlags(body.getCollisionFlags() | 4); // CF_NO_CONTACT_RESPONSE
            }
            body.setDamping(rigidBody.linearDamping, rigidBody.angularDamping);
            body.setSleepingThresholds(0.0, 0.0);
            const bodyPtr = physicsPlugin.bjsAMMO.getPointer(body);
            const heap32 = physicsPlugin.bjsAMMO.HEAP32;
            // ptr + 113 = m_additionalDamping (bool but 4 bytes aligned)
            heap32[bodyPtr / 4 + 113] = 0xFFFFFFFF; // enable additional damping
            nodes[i] = node;
            impostors[i] = impostor;
        }
        const one = Vector3.One();
        const jointRotation = new Quaternion();
        const jointPosition = new Vector3();
        const jointTransform = new Matrix();
        const rigidBodyRotation = new Quaternion();
        const rigidBodyPosition = new Vector3();
        const rigidBodyAInverse = new Matrix();
        const rigidBodyBInverse = new Matrix();
        const jointFinalTransformA = new Matrix();
        const jointFinalTransformB = new Matrix();
        for (let i = 0; i < joints.length; ++i) {
            const joint = joints[i];
            if (joint.rigidbodyIndexA < 0 || rigidBodies.length <= joint.rigidbodyIndexA) {
                logger.warn(`Rigid body index out of range failed to create joint: ${joint.name}`);
                continue;
            }
            if (joint.rigidbodyIndexB < 0 || rigidBodies.length <= joint.rigidbodyIndexB) {
                logger.warn(`Rigid body index out of range failed to create joint: ${joint.name}`);
                continue;
            }
            const bodyA = impostors[joint.rigidbodyIndexA];
            const bodyB = impostors[joint.rigidbodyIndexB];
            if (bodyA === null || bodyB === null) {
                logger.warn(`Rigid body not found failed to create joint: ${joint.name}`);
                continue;
            }
            Matrix.ComposeToRef(one, Quaternion.FromEulerAnglesToRef(joint.rotation[0], joint.rotation[1], joint.rotation[2], jointRotation), jointPosition.copyFromFloats(joint.position[0] * scalingFactor, joint.position[1] * scalingFactor, joint.position[2] * scalingFactor), jointTransform);
            const bodyInfoA = rigidBodies[joint.rigidbodyIndexA];
            const bodyInfoB = rigidBodies[joint.rigidbodyIndexB];
            {
                const shapeRotation = bodyInfoA.shapeRotation;
                const shapePosition = bodyInfoA.shapePosition;
                Matrix.ComposeToRef(one, Quaternion.FromEulerAnglesToRef(shapeRotation[0], shapeRotation[1], shapeRotation[2], rigidBodyRotation), rigidBodyPosition.copyFromFloats(shapePosition[0] * scalingFactor, shapePosition[1] * scalingFactor, shapePosition[2] * scalingFactor), rigidBodyAInverse).invert();
            }
            {
                const shapeRotation = bodyInfoB.shapeRotation;
                const shapePosition = bodyInfoB.shapePosition;
                Matrix.ComposeToRef(one, Quaternion.FromEulerAnglesToRef(shapeRotation[0], shapeRotation[1], shapeRotation[2], rigidBodyRotation), rigidBodyPosition.copyFromFloats(shapePosition[0] * scalingFactor, shapePosition[1] * scalingFactor, shapePosition[2] * scalingFactor), rigidBodyBInverse).invert();
            }
            jointTransform.multiplyToRef(rigidBodyAInverse, jointFinalTransformA);
            jointTransform.multiplyToRef(rigidBodyBInverse, jointFinalTransformB);
            const physicsJoint = new Generic6DofSpringJoint({
                mainFrame: jointFinalTransformA,
                connectedFrame: jointFinalTransformB,
                useLinearReferenceFrameA: true,
                linearLowerLimit: new Vector3(joint.positionMin[0], joint.positionMin[1], joint.positionMin[2]),
                linearUpperLimit: new Vector3(joint.positionMax[0], joint.positionMax[1], joint.positionMax[2]),
                angularLowerLimit: new Vector3(joint.rotationMin[0], joint.rotationMin[1], joint.rotationMin[2]),
                angularUpperLimit: new Vector3(joint.rotationMax[0], joint.rotationMax[1], joint.rotationMax[2]),
                linearStiffness: new Vector3(joint.springPosition[0], joint.springPosition[1], joint.springPosition[2]),
                angularStiffness: new Vector3(joint.springRotation[0], joint.springRotation[1], joint.springRotation[2]),
                collision: true // do not disable collision between the two rigid bodies
            });
            physicsJoint;
            bodyA.addJoint(bodyB, physicsJoint);
            // adjust the physics mode of the rigid bodies
            // ref: https://web.archive.org/web/20140815111315/www20.atpages.jp/katwat/wp/?p=4135
            const nodeA = nodes[joint.rigidbodyIndexA];
            const nodeB = nodes[joint.rigidbodyIndexB];
            if (nodeA.physicsMode !== PmxObject.RigidBody.PhysicsMode.FollowBone &&
                nodeB.physicsMode === PmxObject.RigidBody.PhysicsMode.PhysicsWithBone) { // case: A is parent of B
                if (resolveRigidBodyBone(bodyInfoB).parentBone === resolveRigidBodyBone(bodyInfoA)) {
                    nodeB.physicsMode = PmxObject.RigidBody.PhysicsMode.Physics;
                }
            }
            else if (nodeB.physicsMode !== PmxObject.RigidBody.PhysicsMode.FollowBone &&
                nodeA.physicsMode === PmxObject.RigidBody.PhysicsMode.PhysicsWithBone) { // case: B is parent of A
                if (resolveRigidBodyBone(bodyInfoA).parentBone === resolveRigidBodyBone(bodyInfoB)) {
                    nodeA.physicsMode = PmxObject.RigidBody.PhysicsMode.Physics;
                }
            }
        }
        return new MmdAmmoPhysicsModel(this, nodes, impostors, rootMesh, physicsPlugin.bjsAMMO);
    }
    static _ZeroVector = Vector3.Zero();
    _onAfterPhysics = () => {
        const kinematicOnces = this._kinematicOnces;
        for (let i = 0; i < kinematicOnces.length; ++i) {
            const impostor = kinematicOnces[i];
            impostor.setLinearVelocity(MmdAmmoPhysics._ZeroVector);
            impostor.setAngularVelocity(MmdAmmoPhysics._ZeroVector);
            // eslint-disable-next-line @typescript-eslint/consistent-type-imports
            const body = impostor.physicsBody;
            body.setCollisionFlags(body.getCollisionFlags() & ~2); // CF_KINEMATIC_OBJECT
        }
        kinematicOnces.length = 0;
    };
    /** @internal */
    _makeKinematicOnce(impostor) {
        if (!impostor._options.disableBidirectionalTransformation) {
            return;
        }
        if (this._kinematicOnces.length === 0) {
            this._scene.onAfterPhysicsObservable.addOnce(this._onAfterPhysics);
        }
        this._kinematicOnces.push(impostor);
        impostor.physicsBody.setCollisionFlags(impostor.physicsBody.getCollisionFlags() | 2); // CF_KINEMATIC_OBJECT
    }
}
