
import { Constraint } from './Constraint';
import { Equation } from '../equations/Equation';
import { RotationalVelocityEquation } from '../equations/RotationalVelocityEquation';
import { RotationalLockEquation } from '../equations/RotationalLockEquation'
import { vec2 } from '../math/vec2';
var sub = vec2.subtract
    , add = vec2.add
    , rotate = vec2.rotate
    , dot = vec2.dot
    , copy = vec2.copy
    , crossLength = vec2.crossLength;


var worldPivotA = vec2.create(),
    worldPivotB = vec2.create(),
    xAxis = vec2.fromValues(1, 0),
    yAxis = vec2.fromValues(0, 1),
    g = vec2.create();

export class RevoluteConstraint extends Constraint {

    maxForce: number;
    pivotA: Float32Array;
    pivotB: Float32Array;
    motorEquation: RotationalVelocityEquation;
    upperLimitEquation: RotationalLockEquation;
    lowerLimitEquation: RotationalLockEquation;
    angle: number;
    lowerLimitEnabled: boolean;
    upperLimitEnabled: boolean;
    lowerLimit: number;
    upperLimit: number;


    /**
     * Connects two bodies at given offset points, letting them rotate relative to each other around this point.
     * @class RevoluteConstraint
     * @constructor
     * @author schteppe
     * @param {Body}    bodyA
     * @param {Body}    bodyB
     * @param {Object}  [options]
     * @param {Array}   [options.worldPivot] A pivot point given in world coordinates. If specified, localPivotA and localPivotB are automatically computed from this value.
     * @param {Array}   [options.localPivotA] The point relative to the center of mass of bodyA which bodyA is constrained to.
     * @param {Array}   [options.localPivotB] See localPivotA.
     * @param {Number}  [options.maxForce] The maximum force that should be applied to constrain the bodies.
     * @extends Constraint
     *
     * @example
     *     // This will create a revolute constraint between two bodies with pivot point in between them.
     *     var bodyA = new Body({ mass: 1, position: [-1, 0] });
     *     world.addBody(bodyA);
     *
     *     var bodyB = new Body({ mass: 1, position: [1, 0] });
     *     world.addBody(bodyB);
     *
     *     var constraint = new RevoluteConstraint(bodyA, bodyB, {
     *         worldPivot: [0, 0]
     *     });
     *     world.addConstraint(constraint);
     *
     *     // Using body-local pivot points, the constraint could have been constructed like this:
     *     var constraint = new RevoluteConstraint(bodyA, bodyB, {
     *         localPivotA: [1, 0],
     *         localPivotB: [-1, 0]
     *     });
     */
    constructor(bodyA, bodyB, options) {
        options = options || {};
        super(bodyA, bodyB, Constraint.REVOLUTE, options);

        var maxForce = this.maxForce = options.maxForce !== undefined ? options.maxForce : Number.MAX_VALUE;


        var pivotA = this.pivotA = vec2.create();

        var pivotB = this.pivotB = vec2.create();

        if (options.worldPivot) {
            // Compute pivotA and pivotB
            sub(pivotA, options.worldPivot, bodyA.position);
            sub(pivotB, options.worldPivot, bodyB.position);
            // Rotate to local coordinate system
            rotate(pivotA, pivotA, -bodyA.angle);
            rotate(pivotB, pivotB, -bodyB.angle);
        } else {
            // Get pivotA and pivotB
            if (options.localPivotA) {
                copy(pivotA, options.localPivotA);
            }
            if (options.localPivotB) {
                copy(pivotB, options.localPivotB);
            }
        }

        var motorEquation = this.motorEquation = new RotationalVelocityEquation(bodyA, bodyB);
        motorEquation.enabled = false;

        var upperLimitEquation = this.upperLimitEquation = new RotationalLockEquation(bodyA, bodyB);
        var lowerLimitEquation = this.lowerLimitEquation = new RotationalLockEquation(bodyA, bodyB);
        upperLimitEquation.minForce = lowerLimitEquation.maxForce = 0;

        // Equations to be fed to the solver
        var eqs = this.equations = [
            new Equation(bodyA, bodyB, -maxForce, maxForce),
            new Equation(bodyA, bodyB, -maxForce, maxForce),
            motorEquation,
            upperLimitEquation,
            lowerLimitEquation
        ];

        var x = eqs[0];
        var y = eqs[1];

        x.computeGq = function () {
            rotate(worldPivotA, pivotA, bodyA.angle);
            rotate(worldPivotB, pivotB, bodyB.angle);
            add(g, bodyB.position, worldPivotB);
            sub(g, g, bodyA.position);
            sub(g, g, worldPivotA);
            return dot(g, xAxis);
        };

        y.computeGq = function () {
            rotate(worldPivotA, pivotA, bodyA.angle);
            rotate(worldPivotB, pivotB, bodyB.angle);
            add(g, bodyB.position, worldPivotB);
            sub(g, g, bodyA.position);
            sub(g, g, worldPivotA);
            return dot(g, yAxis);
        };

        y.minForce = x.minForce = -maxForce;
        y.maxForce = x.maxForce = maxForce;

        // These never change but the angular parts do
        x.G[0] = -1;
        x.G[1] = 0;

        x.G[3] = 1;
        x.G[4] = 0;

        y.G[0] = 0;
        y.G[1] = -1;

        y.G[3] = 0;
        y.G[4] = 1;

        /**
         * The constraint position.
         * @property angle
         * @type {Number}
         * @readOnly
         */
        this.angle = 0;

        /**
         * Set to true to enable lower limit
         * @property lowerLimitEnabled
         * @type {Boolean}
         */
        this.lowerLimitEnabled = false;

        /**
         * Set to true to enable upper limit
         * @property upperLimitEnabled
         * @type {Boolean}
         */
        this.upperLimitEnabled = false;

        /**
         * The lower limit on the constraint angle.
         * @property lowerLimit
         * @type {Boolean}
         */
        this.lowerLimit = 0;

        /**
         * The upper limit on the constraint angle.
         * @property upperLimit
         * @type {Boolean}
         */
        this.upperLimit = 0;
    }


    /**
     * Set the constraint angle limits, and enable them.
     * @method setLimits
     * @param {number} lower Lower angle limit.
     * @param {number} upper Upper angle limit.
     */
    setLimits(lower, upper) {
        this.lowerLimit = lower;
        this.upperLimit = upper;
        this.lowerLimitEnabled = this.upperLimitEnabled = true;
    };

    update() {
        var bodyA = this.bodyA,
            bodyB = this.bodyB,
            pivotA = this.pivotA,
            pivotB = this.pivotB,
            eqs = this.equations,
            x = eqs[0],
            y = eqs[1],
            upperLimit = this.upperLimit,
            lowerLimit = this.lowerLimit,
            upperLimitEquation = this.upperLimitEquation,
            lowerLimitEquation = this.lowerLimitEquation;

        var relAngle = this.angle = bodyB.angle - bodyA.angle;

        upperLimitEquation.angle = upperLimit;
        upperLimitEquation.enabled = this.upperLimitEnabled && relAngle > upperLimit;

        lowerLimitEquation.angle = lowerLimit;
        lowerLimitEquation.enabled = this.lowerLimitEnabled && relAngle < lowerLimit;

        /*
    
        The constraint violation is
    
            g = xj + rj - xi - ri
    
        ...where xi and xj are the body positions and ri and rj world-oriented offset vectors. Differentiate:
    
            gdot = vj + wj x rj - vi - wi x ri
    
        We split this into x and y directions. (let x and y be unit vectors along the respective axes)
    
            gdot * x = ( vj + wj x rj - vi - wi x ri ) * x
                     = ( vj*x + (wj x rj)*x -vi*x -(wi x ri)*x
                     = ( vj*x + (rj x x)*wj -vi*x -(ri x x)*wi
                     = [ -x   -(ri x x)   x   (rj x x)] * [vi wi vj wj]
                     = G*W
    
        ...and similar for y. We have then identified the jacobian entries for x and y directions:
    
            Gx = [ x   (rj x x)   -x   -(ri x x)]
            Gy = [ y   (rj x y)   -y   -(ri x y)]
    
        So for example, in the X direction we would get in 2 dimensions
    
            G = [ [1   0   (rj x [1,0])   -1   0   -(ri x [1,0])]
                  [0   1   (rj x [0,1])    0  -1   -(ri x [0,1])]
         */

        rotate(worldPivotA, pivotA, bodyA.angle);
        rotate(worldPivotB, pivotB, bodyB.angle);

        // @todo: these are a bit sparse. We could save some computations on making custom eq.computeGW functions, etc

        var xG = x.G;
        xG[2] = -crossLength(worldPivotA, xAxis);
        xG[5] = crossLength(worldPivotB, xAxis);

        var yG = y.G;
        yG[2] = -crossLength(worldPivotA, yAxis);
        yG[5] = crossLength(worldPivotB, yAxis);
    };

    get motorEnabled(): boolean {
        return this.motorEquation.enabled;
    }
    set motorEnabled(value: boolean) {
        this.motorEquation.enabled = value;
    }

    get motorSpeed() {
        return this.motorEquation.relativeVelocity;
    }
    set motorSpeed(value) {
        this.motorEquation.relativeVelocity = value;
    }

    get motorMaxForce() {
        return this.motorEquation.maxForce;
    }
    set motorMaxForce(value) {
        var eq = this.motorEquation;
        eq.maxForce = value;
        eq.minForce = -value;
    }



    /**
     * Enable the rotational motor
     * @deprecated Use motorEnabled instead
     * @method enableMotor
     */
    enableMotor() {
        console.warn("revolute.enableMotor() is deprecated, do revolute.motorEnabled = true; instead.");
        this.motorEnabled = true;
    };

    /**
     * Disable the rotational motor
     * @deprecated Use motorEnabled instead
     * @method disableMotor
     */
    disableMotor() {
        console.warn("revolute.disableMotor() is deprecated, do revolute.motorEnabled = false; instead.");
        this.motorEnabled = false;
    };

    /**
     * Check if the motor is enabled.
     * @method motorIsEnabled
     * @deprecated Use motorEnabled instead
     * @return {Boolean}
     */
    motorIsEnabled() {
        console.warn("revolute.motorIsEnabled() is deprecated, use revolute.motorEnabled instead.");
        return this.motorEnabled;
    };

    /**
     * Set the speed of the rotational constraint motor
     * @method setMotorSpeed
     * @deprecated Use .motorSpeed instead
     * @param {Number} speed
     */
    setMotorSpeed(speed) {
        console.warn("revolute.setMotorSpeed(speed) is deprecated, do revolute.motorSpeed = speed; instead.");
        this.motorSpeed = speed;
    };

    /**
     * Get the speed of the rotational constraint motor
     * @deprecated Use .motorSpeed instead
     * @method getMotorSpeed
     * @return {Number}
     */
    getMotorSpeed() {
        console.warn("revolute.getMotorSpeed() is deprecated, use revolute.motorSpeed instead.");
        return this.motorSpeed;
    };
}
