import { Ammo } from './utils.js';

import {
    BODYFLAG_KINEMATIC_OBJECT, BODYTYPE_STATIC,
    BODYGROUP_DYNAMIC, BODYGROUP_KINEMATIC, BODYGROUP_STATIC,
    BODYMASK_ALL, BODYMASK_NOT_STATIC,
    BODYSTATE_ACTIVE_TAG, BODYSTATE_DISABLE_DEACTIVATION, BODYSTATE_DISABLE_SIMULATION,
    BODYTYPE_DYNAMIC, BODYTYPE_KINEMATIC
} from './constants.js';

import { Vec3 } from "./Vec3.js";
import { Quaternion } from './Quaternion.js';

export class RigidBody {

    constructor(entity, options = {}) {

        this.entity = entity;

        entity.userData.body = this;

        this._body = null;
        this._mass = options.mass !== undefined ? options.mass : 1;

        this._type = BODYTYPE_STATIC;

        this._friction = 0.5;
        this._restitution = 0;

        let shape = options.shape !== undefined ? options.shape : null;
        this._shape = shape._shape;

        this._position = options.position !== undefined ? options.position : new Vec3();
        this._quaternion = options.quaternion !== undefined ? options.quaternion : new Quaternion();

        this._angularDamping = 0;
        this._angularFactor = new Vec3(1, 1, 1);
        this._angularVelocity = new Vec3();

        this._linearDamping = 0;
        this._linearFactor = new Vec3(1, 1, 1);
        this._linearVelocity = new Vec3();

        this._type = BODYTYPE_DYNAMIC;

        this._animated = false;

    }

    get animated() {
        return this._animated;
    }

    set animated(value) {

        if (this._animated !== value) {
            this._animated = value;


        }

    }


    get type() {
        return this._type;
    }

    set type(type) {
        if (this._type !== type) {
            this._type = type;

            // this.disableSimulation();

            // set group and mask to defaults for type
            switch (type) {
                case BODYTYPE_DYNAMIC:
                    this._group = BODYGROUP_DYNAMIC;
                    // this._mask = BODYMASK_ALL;
                    break;
                case BODYTYPE_KINEMATIC:
                    this._group = BODYGROUP_KINEMATIC;
                    // this._mask = BODYMASK_ALL;
                    break;
                case BODYTYPE_STATIC:
                default:
                    this._group = BODYGROUP_STATIC;
                    // this._mask = BODYMASK_NOT_STATIC;
                    break;
            }

            // Create a new body
            // this.createBody();
        }
    }


    get mass() {
        return this._mass;
    }

    set mass(mass) {
        if (this._mass !== mass) {
            this._mass = mass;

            if (this._body && this._type === BODYTYPE_DYNAMIC) {
                const enabled = this.enabled && this.entity.enabled;
                if (enabled) {
                    this.disableSimulation();
                }

                let localInertia = new Ammo.btVector3(0, 0, 0);

                if (mass)
                    /* calculate new inertia if non-zero mass */
                    // calculateLocalInertia writes local inertia to localInertia here...
                    this._body.getCollisionShape().calculateLocalInertia(mass, localInertia);

                // ...and then writes the calculated local inertia to the body
                this._body.setMassProps(mass, localInertia);
                this._body.updateInertiaTensor();

                if (enabled) {
                    // this.enableSimulation();
                }
            }
        }
    }


    get position() {
        return this._position;
    }

    set position(loc) {
        this.setTransform(loc, this.quaternion);
    }

    get quaternion() {
        return this._quaternion;
    }

    set quaternion(rot) {
        this.setTransform(this.position, rot);
    }

    _getTransform() {

        const pos = this.position;
        const rot = this.quaternion;

        let transform = new Ammo.btTransform();
        transform.setIdentity();
        transform.setOrigin(new Ammo.btVector3(pos.x, pos.y, pos.z));
        transform.setRotation(new Ammo.btQuaternion(rot.x, rot.y, rot.z, rot.w));

        return transform;
    }

    setTransform(loc, rot) {

        this._position.copy(loc);
        this._quaternion.copy(rot);

        if (!this._body)
            return;

        let ms = this._body.getMotionState();

        let trans = new Ammo.btTransform();
        trans.setIdentity();
        trans.setOrigin(new Ammo.btVector3(loc.x, loc.y, loc.z));
        trans.setRotation(new Ammo.btQuaternion(rot.x, rot.y, rot.z, rot.w));

        if (this.mass == 0 || this._type === BODYTYPE_KINEMATIC) {
            ms.setWorldTransform(trans);
            return;
        }

        this._body.setWorldTransform(trans);

    }


    // -------------

    get angularDamping() {
        return this._angularDamping;
    }

    set angularDamping(damping) {
        if (this._angularDamping !== damping) {
            this._angularDamping = damping;

            if (this._body) {
                this._body.setDamping(this._linearDamping, damping);
            }
        }
    }


    get angularFactor() {
        return this._angularFactor;
    }

    set angularFactor(factor) {
        if (!this._angularFactor.equals(factor)) {
            this._angularFactor.copy(factor);

            if (this._body && this._type === BODYTYPE_DYNAMIC) {
                let vec = new Ammo.btVector3(factor.x, factor.y, factor.z);
                this._body.setAngularFactor(vec);
            }
        }
    }


    get angularVelocity() {
        if (this._body && this._type === BODYTYPE_DYNAMIC) {
            const velocity = this._body.getAngularVelocity();
            this._angularVelocity.set(velocity.x(), velocity.y(), velocity.z());
        }
        return this._angularVelocity;
    }

    set angularVelocity(velocity) {
        if (this._body && this._type === BODYTYPE_DYNAMIC) {
            this._body.activate();

            let vec = new Ammo.btVector3(velocity.x, velocity.y, velocity.z);
            this._body.setAngularVelocity(vec);

            this._angularVelocity.copy(velocity);
        }
    }


    get linearDamping() {
        return this._linearDamping;
    }

    set linearDamping(damping) {
        if (this._linearDamping !== damping) {
            this._linearDamping = damping;

            if (this._body) {
                this._body.setDamping(damping, this._angularDamping);
            }
        }
    }

    get linearFactor() {
        return this._linearFactor;
    }

    set linearFactor(factor) {
        if (!this._linearFactor.equals(factor)) {
            this._linearFactor.copy(factor);

            if (this._body && this._type === BODYTYPE_DYNAMIC) {
                let vec = new Ammo.btVector3(factor.x, factor.y, factor.z);
                this._body.setLinearFactor(vec);
            }
        }
    }

    get linearVelocity() {
        if (this._body && this._type === BODYTYPE_DYNAMIC) {
            const velocity = this._body.getLinearVelocity();
            this._linearVelocity.set(velocity.x(), velocity.y(), velocity.z());
        }
        return this._linearVelocity;
    }

    set linearVelocity(velocity) {
        if (this._body && this._type === BODYTYPE_DYNAMIC) {
            this._body.activate();

            let vec = new Ammo.btVector3(velocity.x, velocity.y, velocity.z);
            this._body.setLinearVelocity(vec);

            this._linearVelocity.copy(velocity);
        }
    }


    get friction() {
        return this._friction;
    }

    set friction(friction) {
        if (this._friction !== friction) {
            this._friction = friction;

            if (this._body) {
                this._body.setFriction(friction);
            }
        }
    }


    get restitution() {
        return this._restitution;
    }

    set restitution(restitution) {
        if (this._restitution !== restitution) {
            this._restitution = restitution;

            if (this._body) {
                this._body.setRestitution(restitution);
            }
        }
    }

    setKinematicState(kinematic) {
        let body = this._body;

        if (kinematic) {
            this.type = BODYTYPE_KINEMATIC;
            body.setCollisionFlags(body.getCollisionFlags() | BODYFLAG_KINEMATIC_OBJECT);
        }
        else {
            this.type = BODYTYPE_DYNAMIC;
            body.setCollisionFlags(body.getCollisionFlags() & ~BODYFLAG_KINEMATIC_OBJECT);
        }

    }

    setActivationState(use_deactivation) {
        let body = this._body;

        if (use_deactivation) {
            body.forceActivationState(BODYSTATE_ACTIVE_TAG);
        }
        else {
            body.setActivationState(BODYSTATE_DISABLE_DEACTIVATION);
        }
    }

    isActive() {
        return this._body ? this._body.isActive() : false;
    }

    /* activate() {
        if (this._body) {
            this._body.activate();
        }
    } */

    activate() {
        this._body.setActivationState(BODYSTATE_ACTIVE_TAG);
    }
    deactivate() {
        this._body.setActivationState(BODYSTATE_ISLAND_SLEEPING);
    }


    /**
     * @private
     * @function
     * @name RigidBodyComponent#_getEntityTransform
     * @description Writes an entity transform into an Ammo.btTransform but ignoring scale.
     * @return {Ammo.btTransform} transform - The ammo transform to write the entity transform to.
     */
    _getEntityTransform() {

        let transform = new Ammo.btTransform();
        transform.setIdentity();

        const entity = this.entity;
        const pos = entity.position;
        const rot = entity.rotation;

        transform.setOrigin(new Ammo.btVector3(pos.x, pos.y, pos.z));
        transform.setRotation(new Ammo.btQuaternion(rot.x, rot.y, rot.z, rot.w));

        return transform;

    }

    _setActivationState(use_deactivation) {
        if (use_deactivation) {
            this._body.forceActivationState(ACTIVE_TAG);
        }
        else {
            this._body.setActivationState(DISABLE_DEACTIVATION);
        }
    }

    setActivationState(newState) {
        this._body.setActivationState(newState);
    }


    /**
     * @private
     * @function
     * @name RigidBodyComponent#createBody
     * @description If the Entity has a Collision shape attached then create 
     * a rigid body using this shape. This method destroys the existing body.
     */
    createBody() {
        const entity = this.entity;
        let shape = this._shape;

        if (shape) {
            // if (this._body)
            // this.system.onRemove(entity, this);

            const mass = this._type === BODYTYPE_DYNAMIC ? this._mass : 0;

            let transform = this._getTransform();

            // const body = this.system.createBody(mass, shape, transform);
            // crete body
            const localInertia = new Ammo.btVector3(0, 0, 0);
            if (mass !== 0) {
                shape.calculateLocalInertia(mass, localInertia);
            }

            const motionState = new Ammo.btDefaultMotionState(transform);
            const bodyInfo = new Ammo.btRigidBodyConstructionInfo(mass, motionState, shape, localInertia);
            const body = new Ammo.btRigidBody(bodyInfo);
            Ammo.destroy(bodyInfo);
            Ammo.destroy(localInertia);

            body.setRestitution(this._restitution);
            body.setFriction(this._friction);
            body.setRollingFriction(this._rollingFriction);
            body.setDamping(this._linearDamping, this._angularDamping);

            let vec = new Ammo.btVector3();

            if (this._type === BODYTYPE_DYNAMIC) {
                const linearFactor = this._linearFactor;
                vec.setValue(linearFactor.x, linearFactor.y, linearFactor.z);
                body.setLinearFactor(vec);

                const angularFactor = this._angularFactor;
                vec.setValue(angularFactor.x, angularFactor.y, angularFactor.z);
                body.setAngularFactor(vec);
            } else if (this._type === BODYTYPE_KINEMATIC) {
                body.setCollisionFlags(body.getCollisionFlags() | BODYFLAG_KINEMATIC_OBJECT);
                body.setActivationState(BODYSTATE_DISABLE_DEACTIVATION);
            }

            body.entity = entity;

            this._body = body;

            if (this.enabled && entity.enabled) {
                // this.enableSimulation();
            }

        }
    }

    addToWorld(world) {
        world.addBody(this);
        return this;
    }

    /* Sync rigid body and object transformations */
    update(object3D) {

        let trans = new Ammo.btTransform();
        const ms = this._body.getMotionState();

        if (ms) {

            ms.getWorldTransform(trans);
            const p = trans.getOrigin();
            const q = trans.getRotation();
            object3D.position.set(p.x(), p.y(), p.z());
            object3D.quaternion.set(q.x(), q.y(), q.z(), q.w());

        }

    }

    /**
     * 
     * @param {World} world Physics world
     */
    static createSubstepData(world) {

        /* Objects that we want to update substep location/rotation for. */
        let substepTargets = [];

        world.bodies.forEach((body) => {

            if (body.type == BODYTYPE_KINEMATIC) {

                let loc = new Vec3(), rot = new Quaternion(), scale = new Vec3();

                let data = new KinematicSubstepData("RigidBody Substep data");

                data.body = body;

                loc.copy(body.position);
                rot.copy(body.quaternion);

                data.old_pos.copy(loc);
                data.old_rot.copy(rot);

                loc.copy(body.entity.position);
                rot.copy(body.entity.quaternion);

                data.new_pos.copy(loc);
                data.new_rot.copy(rot);

                substepTargets.push(data);

            }

        });

        return substepTargets;

    }

    /**
     * @description Kinematic object update
     * @param {*} targets Kinematic type object array need update
     * @param {*} interpFac interpolation factor
     */

    static updateKinematic(datas = [], interpFac = 1) {

        if (datas.length == 0)
            return;

        datas.forEach((data) => {

            let loc = new Vec3(), rot = new Quaternion();

            loc.lerpVectors(data.old_pos, data.new_pos, interpFac);
            rot.lerpQuaternions(data.old_rot, data.new_rot, interpFac);

            data.body.activate();
            data.body.setTransform(loc, rot);

        });
    }

}


class KinematicSubstepData {
    constructor(name) {
        this.name = name || '';
        this.body;
        this.old_pos = new Vec3();
        this.new_pos = new Vec3();
        this.old_rot = new Quaternion();
        this.new_rot = new Quaternion();
        this.scale_changed = false;
        this.old_scale = new Vec3();
        this.new_scale = new Vec3();
    }
}

function isAffectedBySimulation(body) {
    /* Check if the object will have its transform changed by the rigidbody simulation. */

    if (body.type == BODYTYPE_KINEMATIC) {
        return false;
    }

    return true;
}