import { Utils } from "../utils/Utils"
import { Body } from "../objects/Body";
import { vec2 } from '../math/vec2';
var scale = vec2.scale,
    multiply = vec2.multiply,
    createVec2 = vec2.create;

var qi = createVec2(),
    qj = createVec2(),
    iMfi = createVec2(),
    iMfj = createVec2();

export class Equation {

    /**
     * The default stiffness when creating a new Equation.
     * @static
     * @property {Number} DEFAULT_STIFFNESS
     * @default 1e6
     */
    static DEFAULT_STIFFNESS = 1e6;

    /**
     * The default relaxation when creating a new Equation.
     * @static
     * @property {Number} DEFAULT_RELAXATION
     * @default 4
     */
    static DEFAULT_RELAXATION = 4;

    /**
     * Minimum force to apply when solving.
     * @property minForce
     * @type {Number}
     */
    minForce: number;
    /**
     * Max force to apply when solving.
     * @property maxForce
     * @type {Number}
     */
    maxForce: number;
    /**
     * Cap the constraint violation (G*q) to this value.
     * @property maxBias
     * @type {Number}
     */
    maxBias: number;
    /**
     * First body participating in the constraint
     * @property bodyA
     * @type {Body}
     */
    bodyA: Body;
    /**
     * Second body participating in the constraint
     * @property bodyB
     * @type {Body}
     */
    bodyB: Body;
    /**
     * The stiffness of this equation. Typically chosen to a large number (~1e7), but can be chosen somewhat freely to get a stable simulation.
     * @property stiffness
     * @type {Number}
     */
    stiffness: number;
    /**
     * The number of time steps needed to stabilize the constraint equation. Typically between 3 and 5 time steps.
     * @property relaxation
     * @type {Number}
     */
    relaxation: number;
    /**
     * The Jacobian entry of this equation. 6 numbers, 3 per body (x,y,angle).
     * @property G
     * @type {Array}
     */
    G: Float32Array;

    offset: number;
    a: number;
    b: number;
    epsilon: number;
    timeStep: number;
    /**
     * Indicates if stiffness or relaxation was changed.
     * @property {Boolean} needsUpdate
     */
    needsUpdate: boolean;
    /**
     * The resulting constraint multiplier from the last solve. This is mostly equivalent to the force produced by the constraint.
     * @property multiplier
     * @type {Number}
     */
    multiplier: number;
    /**
     * Relative velocity.
     * @property {Number} relativeVelocity
     */
    relativeVelocity: number;
    /**
     * Whether this equation is enabled or not. If true, it will be added to the solver.
     * @property {Boolean} enabled
     */
    enabled: boolean;
    lambda: number;
    B: number;
    invC: number;
    minForceDt: number;
    maxForceDt: number;
    index: number;


    /**
     * 约束方程基类
     * Base class for constraint equations.
     * @class Equation
     * @constructor
     * @param {Body} bodyA First body participating in the equation
     * @param {Body} bodyB Second body participating in the equation
     * @param {number} minForce Minimum force to apply. Default: -Number.MAX_VALUE
     * @param {number} maxForce Maximum force to apply. Default: Number.MAX_VALUE
     */
    constructor(bodyA?: Body, bodyB?: Body, minForce?: number, maxForce?: number) {

        this.minForce = minForce === undefined ? -Number.MAX_VALUE : minForce;


        this.maxForce = maxForce === undefined ? Number.MAX_VALUE : maxForce;


        this.maxBias = Number.MAX_VALUE;


        this.bodyA = bodyA;


        this.bodyB = bodyB;


        this.stiffness = Equation.DEFAULT_STIFFNESS;


        this.relaxation = Equation.DEFAULT_RELAXATION;


        this.G = new Utils.ARRAY_TYPE(6);
        for (var i = 0; i < 6; i++) {
            this.G[i] = 0;
        }

        this.offset = 0;

        this.a = 0;
        this.b = 0;
        this.epsilon = 0;
        this.timeStep = 1 / 60;


        this.needsUpdate = true;


        this.multiplier = 0;


        this.relativeVelocity = 0;


        this.enabled = true;

        // Temp stuff
        this.lambda = this.B = this.invC = this.minForceDt = this.maxForceDt = 0;
        this.index = -1;
    }


    /**
     * Compute SPOOK parameters .a, .b and .epsilon according to the current parameters. See equations 9, 10 and 11 in the <a href="http://www8.cs.umu.se/kurser/5DV058/VT09/lectures/spooknotes.pdf">SPOOK notes</a>.
     * @method update
     */
    update() {
        var k = this.stiffness,//1111111
            d = this.relaxation,//4
            h = this.timeStep;//1/60

        this.a = 4 / (h * (1 + 4 * d));
        this.b = (4 * d) / (1 + 4 * d);
        this.epsilon = 4 / (h * h * k * (1 + 4 * d));

        this.needsUpdate = false;
    };

    /**
     * Multiply a jacobian entry with corresponding positions or velocities
     * @method gmult
     * @return {Number}
     */
    gmult(G, vi, wi, vj, wj) {
        return G[0] * vi[0] +
            G[1] * vi[1] +
            G[2] * wi +
            G[3] * vj[0] +
            G[4] * vj[1] +
            G[5] * wj;
    };

    /**
     * Computes the RHS of the SPOOK equation
     * @method computeB
     * @return {Number}
     */
    computeB(a, b, h) {
        var GW = this.computeGW();
        var Gq = this.computeGq();
        var maxBias = this.maxBias;
        if (Math.abs(Gq) > maxBias) {
            Gq = Gq > 0 ? maxBias : -maxBias;
        }
        var GiMf = this.computeGiMf();
        var B = - Gq * a - GW * b - GiMf * h;
        return B;
    };

    /**
     * Computes G\*q, where q are the generalized body coordinates
     * @method computeGq
     * @return {Number}
     */
    computeGq() {
        var G = this.G,
            bi = this.bodyA,
            bj = this.bodyB,
            ai = bi.angle,
            aj = bj.angle;

        return this.gmult(G, qi, ai, qj, aj) + this.offset;
    }

    /**
     * Computes G\*W, where W are the body velocities
     * @method computeGW
     * @return {Number}
     */
    computeGW() {
        var G = this.G,
            bi = this.bodyA,
            bj = this.bodyB,
            vi = bi.velocity,
            vj = bj.velocity,
            wi = bi.angularVelocity,
            wj = bj.angularVelocity;
        return this.gmult(G, vi, wi, vj, wj) + this.relativeVelocity;
    }

    /**
     * Computes G\*Wlambda, where W are the body velocities
     * @method computeGWlambda
     * @return {Number}
     */
    computeGWlambda() {
        var G = this.G,
            bi = this.bodyA,
            bj = this.bodyB,
            vi = bi.vlambda,
            vj = bj.vlambda,
            wi = bi.wlambda,
            wj = bj.wlambda;
        return this.gmult(G, vi, wi, vj, wj);
    }

    /**
     * Computes G\*inv(M)\*f, where M is the mass matrix with diagonal blocks for each body, and f are the forces on the bodies.
     * @method computeGiMf
     * @return {Number}
     */
    computeGiMf() {
        var bi = this.bodyA,
            bj = this.bodyB,
            fi = bi.force,
            ti = bi.angularForce,
            fj = bj.force,
            tj = bj.angularForce,
            invMassi = bi.invMassSolve,
            invMassj = bj.invMassSolve,
            invIi = bi.invInertiaSolve,
            invIj = bj.invInertiaSolve,
            G = this.G;

        scale(iMfi, fi, invMassi);
        multiply(iMfi, bi.massMultiplier, iMfi);
        scale(iMfj, fj, invMassj);
        multiply(iMfj, bj.massMultiplier, iMfj);

        return this.gmult(G, iMfi, ti * invIi, iMfj, tj * invIj);
    }

    /**
     * Computes G\*inv(M)\*G'
     * @method computeGiMGt
     * @return {Number}
     */
    computeGiMGt() {
        var bi = this.bodyA,
            bj = this.bodyB,
            invMassi = bi.invMassSolve,
            invMassj = bj.invMassSolve,
            invIi = bi.invInertiaSolve,
            invIj = bj.invInertiaSolve,
            G = this.G;

        return G[0] * G[0] * invMassi * bi.massMultiplier[0] +
            G[1] * G[1] * invMassi * bi.massMultiplier[1] +
            G[2] * G[2] * invIi +
            G[3] * G[3] * invMassj * bj.massMultiplier[0] +
            G[4] * G[4] * invMassj * bj.massMultiplier[1] +
            G[5] * G[5] * invIj;
    }

    /**
     * Add constraint velocity to the bodies.
     * @method addToWlambda
     * @param {Number} deltalambda
     */
    addToWlambda(deltalambda) {
        var bi = this.bodyA,
            bj = this.bodyB,
            invMassi = bi.invMassSolve,
            invMassj = bj.invMassSolve,
            invIi = bi.invInertiaSolve,
            invIj = bj.invInertiaSolve,
            G = this.G;

        // v_lambda = G * inv(M) * delta_lambda

        addToVLambda(bi.vlambda, G[0], G[1], invMassi, deltalambda, bi.massMultiplier);
        bi.wlambda += invIi * G[2] * deltalambda;

        addToVLambda(bj.vlambda, G[3], G[4], invMassj, deltalambda, bj.massMultiplier);
        bj.wlambda += invIj * G[5] * deltalambda;
    }

    /**
     * Compute the denominator part of the SPOOK equation: C = G\*inv(M)\*G' + eps
     * @method computeInvC
     * @param  {Number} eps
     * @return {Number}
     */
    computeInvC(eps) {
        var invC = 1 / (this.computeGiMGt() + eps);
        return invC;
    }
};

function addToVLambda(vlambda, Gx, Gy, invMass, deltalambda, massMultiplier) {
    vlambda[0] += Gx * invMass * deltalambda * massMultiplier[0];
    vlambda[1] += Gy * invMass * deltalambda * massMultiplier[1];
}