import { Matrix4, Object3D, Quaternion, Vector3, Vector3Like } from "three";

import { isDevEnvironment } from "../engine/debug/index.js";
import { MODULES } from "../engine/engine_modules.js";
import { CollisionDetectionMode, RigidbodyConstraints } from "../engine/engine_physics.types.js";
import { serializable } from "../engine/engine_serialization_decorator.js";
import { Context, FrameEvent } from "../engine/engine_setup.js";
import { getWorldPosition } from "../engine/engine_three_utils.js";
import type { IRigidbody, Vec3 } from "../engine/engine_types.js";
import { validate } from "../engine/engine_util_decorator.js";
import { delayForFrames, Watch } from "../engine/engine_utils.js";
import { Behaviour } from "./Component.js";

class TransformWatch {

    get isDirty(): boolean {
        return this.positionChanged || this.rotationChanged;
    }
    positionChanged: boolean = false;
    rotationChanged: boolean = false;

    position?: { x?: number, y?: number, z?: number };
    quaternion?: { _x?: number, _y?: number, _z?: number, _w?: number };

    private _positionKeys: string[] = ["x", "y", "z"];
    private _quaternionKeys: string[] = ["_x", "_y", "_z", "_w"];

    reset(clearPreviousValues: boolean = false) {
        this.positionChanged = false;
        this.rotationChanged = false;
        this.mute = false;

        if (clearPreviousValues) {
            if (this.position)
                for (const key of this._positionKeys)
                    delete this.position[key];
            if (this.quaternion)
                for (const key of this._quaternionKeys)
                    delete this.quaternion[key];
        }
    }

    syncValues() {
        for (const key of this._positionKeys) {
            this.position![key] = this.obj.position[key];
        }
        for (const key of this._quaternionKeys) {
            this.quaternion![key] = this.obj.quaternion[key];
        }
    }

    mute: boolean = false;

    applyValues() {
        // only apply the values that actually changed
        // since we want to still control all the other values via physics 
        if (this.positionChanged && this.position) {
            for (const key of this._positionKeys) {
                const val = this.position[key];
                if (val !== undefined)
                    this.obj.position[key] = val;
            }
        }
        if (this.rotationChanged) {
            if (this.quaternion) {
                for (const key of this._quaternionKeys) {
                    const val = this.quaternion[key];
                    if (val !== undefined)
                        this.obj.quaternion[key] = val;
                }
            }
        }
    }

    readonly context: Context;
    readonly obj: Object3D;
    private _positionWatch?: Watch;
    private _rotationWatch?: Watch;

    constructor(obj: Object3D, context: Context) {
        this.context = context;
        this.obj = obj;
    }

    start(position: boolean, rotation: boolean) {
        this.reset();
        if (position) {
            if (!this._positionWatch)
                this._positionWatch = new Watch(this.obj.position, ["x", "y", "z"]);
            this._positionWatch.apply();
            this.position = {};
            // this.position = this.obj.position.clone();
            this._positionWatch.subscribeWrite((val, prop) => {
                if (this.context.physics.engine?.isUpdating || this.mute) return;
                const prev = this.position![prop];
                if (Math.abs(prev - val) < .00001) return;
                this.position![prop] = val;
                this.positionChanged = true;
            })
        }
        if (rotation) {
            if (!this._rotationWatch)
                this._rotationWatch = new Watch(this.obj.quaternion, ["_x", "_y", "_z", "_w"]);
            this._rotationWatch.apply();
            this.quaternion = {};
            // this.quaternion = this.obj.quaternion.clone();
            this._rotationWatch.subscribeWrite((val, prop) => {
                if (this.context.physics.engine?.isUpdating || this.mute) return;
                const prev = this.quaternion![prop];
                if (Math.abs(prev - val) < .00001) return;
                this.quaternion![prop] = val;
                this.rotationChanged = true;
            })
        }

        // detect changes in the parent matrix
        const original = this.obj.matrixWorld.multiplyMatrices.bind(this.obj.matrixWorld);
        const lastParentMatrix = new Matrix4();
        this.obj.matrixWorld["multiplyMatrices"] = (parent: Matrix4, matrix: Matrix4) => {
            if (this.context.physics.engine?.isUpdating || this.mute) return original(parent, matrix);
            if (!lastParentMatrix.equals(parent)) {
                this.positionChanged = true;
                this.rotationChanged = true;
                lastParentMatrix.copy(parent);
            }
            return original(parent, matrix);;
        }
    }

    stop() {
        this._positionWatch?.revoke();
        this._rotationWatch?.revoke();
    }
}

/**
 * A Rigidbody is used together with a Collider to create physical interactions between objects in the scene. 
 * @category Physics 
 * @group Components
 */
export class Rigidbody extends Behaviour implements IRigidbody {

    get isRigidbody() { return true; }

    /** When true the mass will be automatically calculated by the attached colliders */
    @validate()
    autoMass: boolean = true;

    /** By default the mass will be automatically calculated (see `autoMass`) by the physics engine using the collider sizes  
     * To set the mass manually you can either set the `mass` value or set `autoMass` to `false`
     */
    @serializable()
    set mass(value: number) {
        if (value === this._mass) return;
        this._mass = value;
        this._propertiesChanged = true;
        // When setting the mass manually we disable autoMass
        if (this.__didAwake) {
            this.autoMass = false;
        }
    }
    get mass() {
        if (this.autoMass)
            return this.context.physics.engine?.getBody(this)?.mass() ?? -1;
        return this._mass;
    }
    private _mass: number = 0;

    /**
     * Use gravity is a flag that can be set to false to disable gravity for a specific rigid-body.
     */
    @validate()
    @serializable()
    useGravity: boolean = true;

    /**
     * The center of mass is the point around which the mass of the rigid-body is evenly distributed. It is used to compute the torque applied to the rigid-body when forces are applied to it.
     */
    @serializable(Vector3)
    centerOfMass: Vector3 = new Vector3(0, 0, 0);

    /**
     * Constraints are used to lock the position or rotation of an object in a specific axis.
     */
    @validate()
    @serializable()
    constraints: RigidbodyConstraints = RigidbodyConstraints.None;

    /**
     * IsKinematic is a flag that can be set to true to make a rigid-body kinematic. Kinematic rigid-bodies are not affected by forces and collisions. They are meant to be animated by the user.
     */
    @validate()
    @serializable()
    isKinematic: boolean = false;

    /** Drag is a force that resists the motion of the rigid-body. It is applied to the center-of-mass of the rigid-body. 
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#damping */
    @validate()
    @serializable()
    drag: number = 0;

    /** Angular drag is a force that resists the rotation of the rigid-body. It is applied to the center-of-mass of the rigid-body. 
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#damping */
    @validate()
    @serializable()
    angularDrag: number = 1;

    /**
     * Detect collisions is a flag that can be set to false to disable collision detection for a specific rigid-body.
     */
    @validate()
    @serializable()
    detectCollisions: boolean = true;

    /** The sleeping threshold is the minimum velocity below which a dynamic rigid-body will be put to sleep by the physics engine. 
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#sleeping */
    @validate()
    @serializable()
    sleepThreshold: number = 0.01;

    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#continuous-collision-detection */
    @validate()
    @serializable()
    collisionDetectionMode: CollisionDetectionMode = CollisionDetectionMode.Discrete;

    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    get lockPositionX() {
        return (this.constraints & RigidbodyConstraints.FreezePositionX) !== 0;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    get lockPositionY() {
        return (this.constraints & RigidbodyConstraints.FreezePositionY) !== 0;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    get lockPositionZ() {
        return (this.constraints & RigidbodyConstraints.FreezePositionZ) !== 0;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    get lockRotationX() {
        return (this.constraints & RigidbodyConstraints.FreezeRotationX) !== 0;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    get lockRotationY() {
        return (this.constraints & RigidbodyConstraints.FreezeRotationY) !== 0;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    get lockRotationZ() {
        return (this.constraints & RigidbodyConstraints.FreezeRotationZ) !== 0;
    }

    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    set lockPositionX(v: boolean) {
        if (v) this.constraints |= RigidbodyConstraints.FreezePositionX;
        else this.constraints &= ~RigidbodyConstraints.FreezePositionX;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    set lockPositionY(v: boolean) {
        if (v) this.constraints |= RigidbodyConstraints.FreezePositionY;
        else this.constraints &= ~RigidbodyConstraints.FreezePositionY;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    set lockPositionZ(v: boolean) {
        if (v) this.constraints |= RigidbodyConstraints.FreezePositionZ;
        else this.constraints &= ~RigidbodyConstraints.FreezePositionZ;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    set lockRotationX(v: boolean) {
        if (v) this.constraints |= RigidbodyConstraints.FreezeRotationX;
        else this.constraints &= ~RigidbodyConstraints.FreezeRotationX;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    set lockRotationY(v: boolean) {
        if (v) this.constraints |= RigidbodyConstraints.FreezeRotationY;
        else this.constraints &= ~RigidbodyConstraints.FreezeRotationY;
    }
    /** @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#locking-translationsrotations */
    set lockRotationZ(v: boolean) {
        if (v) this.constraints |= RigidbodyConstraints.FreezeRotationZ;
        else this.constraints &= ~RigidbodyConstraints.FreezeRotationZ;
    }

    /** Gravity is such a common force that it is implemented as a special case (even if it could easily be implemented by the user using force application). Note however that a change of gravity won't automatically wake-up the sleeping bodies so keep in mind that you may want to wake them up manually before a gravity change.   
     * 
     * It is possible to change the way gravity affects a specific rigid-body by setting the rigid-body's gravity scale to a value other than 1.0. The magnitude of the gravity applied to this body will be multiplied by this scaling factor. Therefore, a gravity scale set to 0.0 will disable gravity for the rigid-body whereas a gravity scale set to 2.0 will make it twice as strong. A negative value will flip the direction of the gravity for this rigid-body.
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#gravity
     */
    set gravityScale(val: number) {
        this._gravityScale = val;
    }
    get gravityScale() {
        return this._gravityScale;
    }
    @validate()
    private _gravityScale: number = 1;

    /** Rigidbodies with higher dominance will be immune to forces originating from contacts with rigidbodies of lower dominance.  
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#dominance
     */
    @validate()
    dominanceGroup: number = 0;

    private static tempPosition: Vector3 = new Vector3();
    private _propertiesChanged: boolean = false;
    private _currentVelocity: Vector3 = new Vector3();
    private _smoothedVelocity: Vector3 = new Vector3();
    private _smoothedVelocityGetter: Vector3 = new Vector3();
    private _lastPosition: Vector3 = new Vector3();

    private _watch?: TransformWatch;

    awake() {
        this._watch = undefined;
        this._propertiesChanged = false;
    }

    onEnable() {
        if (!this._watch) {
            this._watch = new TransformWatch(this.gameObject, this.context);
        }
        this._watch.start(true, true);
        this.startCoroutine(this.beforePhysics(), FrameEvent.LateUpdate);
        if (isDevEnvironment()) {
            if (!globalThis["NEEDLE_USE_RAPIER"])
                console.warn(`Rigidbody could not be created: Rapier physics are explicitly disabled.`);
            else {
                MODULES.RAPIER_PHYSICS.ready().then(async () => {
                    await delayForFrames(3);
                    if (!this.context.physics.engine?.getBody(this))
                        console.warn(`Rigidbody could not be created. Ensure \"${this.name}\" has a Collider component.`);
                })
            }
        }
    }

    onDisable() {
        this._watch?.stop();
        this.context.physics.engine?.removeBody(this);
    }

    onDestroy(): void {
        this.context.physics.engine?.removeBody(this);
    }

    onValidate() {
        this._propertiesChanged = true;
    }

    // need to do this right before updating physics to prevent rendered object glitching through physical bodies
    * beforePhysics() {
        while (true) {
            if (this._propertiesChanged) {
                this._propertiesChanged = false;
                this.context.physics.engine?.updateProperties(this);
            }
            if (this._watch?.isDirty) {
                this._watch.mute = true;
                this._watch.applyValues();
                this.context.physics.engine?.updateBody(this, this._watch.positionChanged, this._watch.rotationChanged);
                this._watch.reset();
            }
            else this._watch?.syncValues();
            this.captureVelocity();
            yield;
        }
    }

    /** Teleport the rigidbody to a new position in the world.   
     * Will reset forces before setting the object world position 
     * @param pt The new position to teleport the object to (world space)
     * @param localspace When true the object will be teleported in local space, otherwise in world space
     * */
    public teleport(pt: { x: number, y: number, z: number }, localspace: boolean = true) {
        this._watch?.reset(true);
        if (localspace) this.gameObject.position.set(pt.x, pt.y, pt.z);
        else this.setWorldPosition(pt.x, pt.y, pt.z);
        this.resetForcesAndTorques();
        this.resetVelocities();
    }

    public resetForces(wakeup: boolean = true) {
        this.context.physics.engine?.resetForces(this, wakeup);
    }

    public resetTorques(wakeup: boolean = true) {
        this.context.physics.engine?.resetTorques(this, wakeup);
    }

    public resetVelocities() {
        this.setVelocity(0, 0, 0);
        this.setAngularVelocity(0, 0, 0);
    }

    public resetForcesAndTorques() {
        this.resetForces();
        this.resetTorques();
    }

    /** When a dynamic rigid-body doesn't move (or moves very slowly) during a few seconds, it will be marked as sleeping by the physics pipeline. Rigid-bodies marked as sleeping are no longer simulated by the physics engine until they are woken up. That way the physics engine doesn't waste any computational resources simulating objects that don't actually move. They are woken up automatically whenever another non-sleeping rigid-body starts interacting with them (either with a joint, or with one of its attached colliders generating contacts).  
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#sleeping */
    public wakeUp() {
        this.context.physics.engine?.wakeup(this);
    }

    public get isSleeping() {
        return this.context.physics.engine?.isSleeping(this);
    }

    /** Call to force an update of the rigidbody properties in the physics engine */
    public updateProperties() {
        this._propertiesChanged = false;
        return this.context.physics.engine?.updateProperties(this);
    }

    /** Forces affect the rigid-body's acceleration whereas impulses affect the rigid-body's velocity 
     * the acceleration change is equal to the force divided by the mass:
     * @link see https://rapier.rs/docs/user_guides/javascript/rigid_bodies#forces-and-impulses */
    public applyForce(vec: Vector3 | Vec3, _rel?: Vector3, wakeup: boolean = true) {
        if (this._propertiesChanged) this.updateProperties();
        this.context.physics.engine?.addForce(this, vec, wakeup);
    }

    /** Forces affect the rigid-body's acceleration whereas impulses affect the rigid-body's velocity 
     * the velocity change is equal to the impulse divided by the mass  
     * @link see https://rapier.rs/docs/user_guides/javascript/rigid_bodies#forces-and-impulses */
    public applyImpulse(vec: Vector3 | Vec3, wakeup: boolean = true) {
        if (this._propertiesChanged) this.updateProperties();
        this.context.physics.engine?.applyImpulse(this, vec, wakeup);
    }

    /** @link see https://rapier.rs/docs/user_guides/javascript/rigid_bodies#forces-and-impulses */
    public setForce(x: Vector3 | Vec3 | number, y?: number, z?: number, wakeup: boolean = true) {
        this.context.physics.engine?.resetForces(this, wakeup);
        if (typeof x === "number") {
            y ??= 0;
            z ??= 0;
            this.context.physics.engine?.addForce(this, { x, y, z }, wakeup);
        }
        else {
            this.context.physics.engine?.addForce(this, x, wakeup);
        }
    }

    /** The velocity of a dynamic rigid-body controls how fast it is moving in time. The velocity is applied at the center-of-mass of the rigid-body. This method returns the current linear velocity of the rigid-body. 
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#velocity */
    public getVelocity(): Vector3 {
        const vel = this.context.physics.engine?.getLinearVelocity(this);
        if (!vel) return this._currentVelocity.set(0, 0, 0);
        this._currentVelocity.x = vel.x;
        this._currentVelocity.y = vel.y;
        this._currentVelocity.z = vel.z;
        return this._currentVelocity;
    }

    public setVelocity(x: number | Vector3, y?: number, z?: number, wakeup: boolean = true) {
        if (x instanceof Vector3) {
            const vec = x;
            this.context.physics.engine?.setLinearVelocity(this, vec, wakeup);
            return;
        }
        if (y === undefined || z === undefined) return;
        this.context.physics.engine?.setLinearVelocity(this, { x: x, y: y, z: z }, wakeup);
    }

    /** The velocity of a dynamic rigid-body controls how fast it is moving in time. The velocity is applied at the center-of-mass of the rigid-body. This method returns the current angular velocity of the rigid-body. 
     * @link https://rapier.rs/docs/user_guides/javascript/rigid_bodies#velocity */
    public getAngularVelocity(): Vector3 {
        const vel = this.context.physics.engine?.getAngularVelocity(this);
        if (!vel) return this._currentVelocity.set(0, 0, 0);
        this._currentVelocity.x = vel.x;
        this._currentVelocity.y = vel.y;
        this._currentVelocity.z = vel.z;
        return this._currentVelocity;
    }

    public setAngularVelocity(x: Vec3, wakeup?: boolean);
    public setAngularVelocity(x: number, y: number, z: number, wakeup?: boolean);
    public setAngularVelocity(x: number | Vec3, y?: number | boolean, z?: number, wakeup: boolean = true) {

        if (typeof x === "object") {
            const vec = x;
            this.context.physics.engine?.setAngularVelocity(this, vec, wakeup);
            return;
        }
        if (y === undefined || z === undefined || typeof y === "boolean") {
            console.warn("setAngularVelocity expects either a Vec3 or 3 numbers");
            return;
        }
        this.context.physics.engine?.setAngularVelocity(this, { x: x, y: y, z: z }, wakeup);
    }

    /** Set the angular velocity of a rigidbody (equivalent to calling `setAngularVelocity`) */
    public setTorque(x: Vec3);
    public setTorque(x: number, y: number, z: number);
    public setTorque(x: number | Vec3, y?: number, z?: number) {
        if (typeof x === "number") {
            this.setAngularVelocity(x, y!, z!);
        }
        else this.setAngularVelocity(x);
    }

    /** 
     * Returns the rigidbody velocity smoothed over ~ 10 frames
     */
    public get smoothedVelocity(): Vector3 {
        this._smoothedVelocityGetter.copy(this._smoothedVelocity);
        return this._smoothedVelocityGetter.multiplyScalar(1 / this.context.time.deltaTime);
    }


    /**d
     * @deprecated not used anymore and will be removed in a future update
     */
    public setBodyFromGameObject(_velocity: Vector3 | null | { x: number, y: number, z: number } = null) { }


    private captureVelocity() {
        const matrixWorld = this.gameObject.matrixWorld;
        Rigidbody.tempPosition.setFromMatrixPosition(matrixWorld);
        const vel = Rigidbody.tempPosition.sub(this._lastPosition);
        this._lastPosition.copy(Rigidbody.tempPosition);
        this._smoothedVelocity.lerp(vel, this.context.time.deltaTime / .1);
    }
}

const tempPosition = new Vector3();
const tempQuaternion = new Quaternion();
const tempScale = new Vector3();