#include "coduo_x87emu.h" /* defines EMULATE_X87; x87 shim when it is 1 */

#include "core_math_private.h"

void QuatToMatrix(float quatAndMatrix[9])
{
    float xx;
    float yy;
    float zz;
    float scale;
    float xy;
    float xz;
    float xw;
    float yz;
    float yw;
    float zw;

#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x0806977c): every float op
     * runs through the shim, preserving stock's 80-bit intermediates and
     * read-before-write ordering of the aliased quat/matrix array. The multi-op
     * chains (scale sum, 2/scale, (q2*q3)*scale, and the 1-(a+b) diagonals) are
     * the ones that genuinely need it; the single products are carried through
     * the shim too for a uniform, provably-faithful path. */
    xx = x87f_store_f32(
        x87f_mul(x87f_load_f32(quatAndMatrix[0]), x87f_load_f32(quatAndMatrix[0])));
    yy = x87f_store_f32(
        x87f_mul(x87f_load_f32(quatAndMatrix[1]), x87f_load_f32(quatAndMatrix[1])));
    zz = x87f_store_f32(
        x87f_mul(x87f_load_f32(quatAndMatrix[2]), x87f_load_f32(quatAndMatrix[2])));
    scale = x87f_store_f32(x87f_add(
        x87f_add(x87f_add(x87f_load_f32(xx), x87f_load_f32(yy)), x87f_load_f32(zz)),
        x87f_mul(x87f_load_f32(quatAndMatrix[3]),
                 x87f_load_f32(quatAndMatrix[3]))));

    if (scale != 0.0f) {
        scale = x87f_store_f32(
            x87f_div(x87f_load_f32(2.0f), x87f_load_f32(scale)));
        xx = x87f_store_f32(x87f_mul(x87f_load_f32(xx), x87f_load_f32(scale)));
        yy = x87f_store_f32(x87f_mul(x87f_load_f32(yy), x87f_load_f32(scale)));
        zz = x87f_store_f32(x87f_mul(x87f_load_f32(zz), x87f_load_f32(scale)));

        quatAndMatrix[0] = x87f_store_f32(
            x87f_mul(x87f_load_f32(quatAndMatrix[0]), x87f_load_f32(scale)));
        xy = x87f_store_f32(x87f_mul(x87f_load_f32(quatAndMatrix[0]),
                                     x87f_load_f32(quatAndMatrix[1])));
        xz = x87f_store_f32(x87f_mul(x87f_load_f32(quatAndMatrix[0]),
                                     x87f_load_f32(quatAndMatrix[2])));
        xw = x87f_store_f32(x87f_mul(x87f_load_f32(quatAndMatrix[0]),
                                     x87f_load_f32(quatAndMatrix[3])));

        quatAndMatrix[1] = x87f_store_f32(
            x87f_mul(x87f_load_f32(quatAndMatrix[1]), x87f_load_f32(scale)));
        yz = x87f_store_f32(x87f_mul(x87f_load_f32(quatAndMatrix[1]),
                                     x87f_load_f32(quatAndMatrix[2])));
        yw = x87f_store_f32(x87f_mul(x87f_load_f32(quatAndMatrix[1]),
                                     x87f_load_f32(quatAndMatrix[3])));
        zw = x87f_store_f32(x87f_mul(
            x87f_mul(x87f_load_f32(quatAndMatrix[2]),
                     x87f_load_f32(quatAndMatrix[3])),
            x87f_load_f32(scale)));

        quatAndMatrix[0] = x87f_store_f32(x87f_sub(
            x87f_load_f32(1.0f), x87f_add(x87f_load_f32(yy), x87f_load_f32(zz))));
        quatAndMatrix[1] =
            x87f_store_f32(x87f_add(x87f_load_f32(xy), x87f_load_f32(zw)));
        quatAndMatrix[2] =
            x87f_store_f32(x87f_sub(x87f_load_f32(xz), x87f_load_f32(yw)));
        quatAndMatrix[3] =
            x87f_store_f32(x87f_sub(x87f_load_f32(xy), x87f_load_f32(zw)));
        quatAndMatrix[4] = x87f_store_f32(x87f_sub(
            x87f_load_f32(1.0f), x87f_add(x87f_load_f32(xx), x87f_load_f32(zz))));
        quatAndMatrix[5] =
            x87f_store_f32(x87f_add(x87f_load_f32(yz), x87f_load_f32(xw)));
        quatAndMatrix[6] =
            x87f_store_f32(x87f_add(x87f_load_f32(xz), x87f_load_f32(yw)));
        quatAndMatrix[7] =
            x87f_store_f32(x87f_sub(x87f_load_f32(yz), x87f_load_f32(xw)));
        quatAndMatrix[8] = x87f_store_f32(x87f_sub(
            x87f_load_f32(1.0f), x87f_add(x87f_load_f32(xx), x87f_load_f32(yy))));
    } else {
#else
    xx = quatAndMatrix[0] * quatAndMatrix[0];
    yy = quatAndMatrix[1] * quatAndMatrix[1];
    zz = quatAndMatrix[2] * quatAndMatrix[2];
    scale = (((xx + yy) + zz) + (quatAndMatrix[3] * quatAndMatrix[3]));

    if (scale != 0.0f) {
        scale = 2.0f / scale;
        xx = xx * scale;
        yy = yy * scale;
        zz = zz * scale;

        quatAndMatrix[0] = quatAndMatrix[0] * scale;
        xy = quatAndMatrix[0] * quatAndMatrix[1];
        xz = quatAndMatrix[0] * quatAndMatrix[2];
        xw = quatAndMatrix[0] * quatAndMatrix[3];

        quatAndMatrix[1] = quatAndMatrix[1] * scale;
        yz = quatAndMatrix[1] * quatAndMatrix[2];
        yw = quatAndMatrix[1] * quatAndMatrix[3];
        zw = (quatAndMatrix[2] * quatAndMatrix[3]) * scale;

        quatAndMatrix[0] = 1.0f - (yy + zz);
        quatAndMatrix[1] = xy + zw;
        quatAndMatrix[2] = xz - yw;
        quatAndMatrix[3] = xy - zw;
        quatAndMatrix[4] = 1.0f - (xx + zz);
        quatAndMatrix[5] = yz + xw;
        quatAndMatrix[6] = xz + yw;
        quatAndMatrix[7] = yz - xw;
        quatAndMatrix[8] = 1.0f - (xx + yy);
    } else {
#endif
        quatAndMatrix[0] = 1.0f;
        quatAndMatrix[1] = 0.0f;
        quatAndMatrix[2] = 0.0f;
        quatAndMatrix[3] = 0.0f;
        quatAndMatrix[4] = 1.0f;
        quatAndMatrix[5] = 0.0f;
        quatAndMatrix[6] = 0.0f;
        quatAndMatrix[7] = 0.0f;
        quatAndMatrix[8] = 1.0f;
    }
}
