/// <reference path="vec2.js" />
/// <reference path="mat4.js" />
/// <reference path="circle.js" />
/// <reference path="line2.js" />
/// <reference path="collision.js" />

const GRAVITY = 9.8;
const BYTES_PER_FLOAT = 4;

/**
 * 
 * @param {Shape} pObject a 2D shape like a circle, rectangle, or line
 * @param {Object} pData initial data for the rigidboy
 */
function makeRigidBody2(pObject, pData) {
    pObject.velocity            = vec2();
    pObject.prevVelocity        = pObject.velocity;
    pObject.position            = pData.position || vec2();
    pObject.prevPosition        = pObject.position;
    pObject.force               = vec2();
    pObject.rotationVelocity    = 0;
    pObject.rotationRadians     = 0;
    pObject.torque              = 0;
    pObject.mass                = pData.mass !== undefined ? pData.mass : 1.0;
    pObject.getMomentOfInertia  = pData.getMomentOfInertia;

    return pObject;
}

function updateRigidBody2(pRigidbody, pDeltaTimeSeconds) {
    applyForceRigidbody2(pRigidbody, vec2(0, -1.0 * (pRigidbody.mass * GRAVITY)));
    const lCurrentAcceleration = scaleVec2(pRigidbody.force, 1.0 / pRigidbody.mass);
    pRigidbody.prevVelocity = pRigidbody.velocity;
    pRigidbody.velocity = addVec2(pRigidbody.velocity, scaleVec2(lCurrentAcceleration, pDeltaTimeSeconds));
    pRigidbody.prevPos = { ...pRigidbody.position };
    pRigidbody.position = addVec2(pRigidbody.position, scaleVec2(pRigidbody.velocity, pDeltaTimeSeconds));
    pRigidbody.force = vec2();

    const lMomentOfInertia =  pRigidbody.getMomentOfInertia(pRigidbody);
    const lAngularAcceleration = pRigidbody.torque / lMomentOfInertia;
    pRigidbody.rotationVelocity += lAngularAcceleration * pDeltaTimeSeconds;
    pRigidbody.rotationRadians += pRigidbody.rotationVelocity * pDeltaTimeSeconds;
    pRigidbody.torque = 0;

    pRigidbody.model = rotateMatrix2d(translateMatrix(mat4(), pRigidbody.position.x, pRigidbody.position.y, 0), pRigidbody.rotationRadians);
}

function applyForceRigidbody2(pRigidbody, pForceVector, pPointOfApplication) {
    if (pPointOfApplication !== undefined) {
        const lOriginToPointOfApp = subVec2(vec2(), pPointOfApplication),
            lPerpVec = vec2(-lOriginToPointOfApp.y, lOriginToPointOfApp.x);

            pRigidbody.torque += TORQUE_MULTIPLIER * dot2(lPerpVec, pForceVector);
    }

    pRigidbody.force = addVec2(pRigidbody.force, pForceVector);
}