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

#include "core_math_private.h"

#if EMULATE_X87
/* one 2x2 minor: (a*b) - (c*d) in 80-bit (kept unrounded for the caller). */
#define CODUO_MINOR(a, b, c, d)                                                \
    x87f_sub(x87f_mul(x87f_load_f32(a), x87f_load_f32(b)),                    \
             x87f_mul(x87f_load_f32(c), x87f_load_f32(d)))
#endif

void MatrixInverse3x3(vec3_t in[3], vec3_t out[3])
{
    float reciprocalDeterminant;

#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x080688ac): the determinant
     * is accumulated in 80-bit and rounded to a float slot before the reciprocal
     * divide (it must not stay 80-bit across the divide); each cofactor minor
     * times the reciprocal is an 80-bit chain rounded to float. */
    {
        x87f t0 = x87f_mul(CODUO_MINOR(in[2][2], in[1][1], in[2][1], in[1][2]),
                           x87f_load_f32(in[0][0]));
        x87f t1 = x87f_mul(CODUO_MINOR(in[2][2], in[0][1], in[2][1], in[0][2]),
                           x87f_load_f32(in[1][0]));
        x87f t2 = x87f_mul(CODUO_MINOR(in[1][2], in[0][1], in[1][1], in[0][2]),
                           x87f_load_f32(in[2][0]));
        reciprocalDeterminant = x87f_store_f32(x87f_add(x87f_sub(t0, t1), t2));
    }
    reciprocalDeterminant = x87f_store_f32(
        x87f_div(x87f_load_f32(1.0f), x87f_load_f32(reciprocalDeterminant)));

    out[0][0] = x87f_store_f32(x87f_mul(
        CODUO_MINOR(in[2][2], in[1][1], in[2][1], in[1][2]),
        x87f_load_f32(reciprocalDeterminant)));
    out[0][1] = x87f_store_f32(x87f_mul(
        x87f_neg(CODUO_MINOR(in[2][2], in[0][1], in[2][1], in[0][2])),
        x87f_load_f32(reciprocalDeterminant)));
    out[0][2] = x87f_store_f32(x87f_mul(
        CODUO_MINOR(in[1][2], in[0][1], in[1][1], in[0][2]),
        x87f_load_f32(reciprocalDeterminant)));
    out[1][0] = x87f_store_f32(x87f_mul(
        x87f_neg(CODUO_MINOR(in[2][2], in[1][0], in[2][0], in[1][2])),
        x87f_load_f32(reciprocalDeterminant)));
    out[1][1] = x87f_store_f32(x87f_mul(
        CODUO_MINOR(in[2][2], in[0][0], in[2][0], in[0][2]),
        x87f_load_f32(reciprocalDeterminant)));
    out[1][2] = x87f_store_f32(x87f_mul(
        x87f_neg(CODUO_MINOR(in[1][2], in[0][0], in[1][0], in[0][2])),
        x87f_load_f32(reciprocalDeterminant)));
    out[2][0] = x87f_store_f32(x87f_mul(
        CODUO_MINOR(in[2][1], in[1][0], in[2][0], in[1][1]),
        x87f_load_f32(reciprocalDeterminant)));
    out[2][1] = x87f_store_f32(x87f_mul(
        x87f_neg(CODUO_MINOR(in[2][1], in[0][0], in[2][0], in[0][1])),
        x87f_load_f32(reciprocalDeterminant)));
    out[2][2] = x87f_store_f32(x87f_mul(
        CODUO_MINOR(in[1][1], in[0][0], in[1][0], in[0][1]),
        x87f_load_f32(reciprocalDeterminant)));
#else
    /* stock 0x8068931: the determinant is rounded to a float slot before
     * the reciprocal divide (fld1/fdiv reloads and re-stores the same slot
     * at 0x8068934..0x8068939), so it must not stay in an 80-bit register
     * across the divide. */
    reciprocalDeterminant =
        (((in[2][2] * in[1][1]) - (in[2][1] * in[1][2])) *
             in[0][0] -
         ((in[2][2] * in[0][1]) - (in[2][1] * in[0][2])) *
             in[1][0]) +
        ((in[1][2] * in[0][1]) - (in[1][1] * in[0][2])) *
            in[2][0];
    reciprocalDeterminant = 1.0f / reciprocalDeterminant;

    out[0][0] =
        ((in[2][2] * in[1][1]) - (in[2][1] * in[1][2])) *
        reciprocalDeterminant;
    out[0][1] =
        -((in[2][2] * in[0][1]) - (in[2][1] * in[0][2])) *
        reciprocalDeterminant;
    out[0][2] =
        ((in[1][2] * in[0][1]) - (in[1][1] * in[0][2])) *
        reciprocalDeterminant;
    out[1][0] =
        -((in[2][2] * in[1][0]) - (in[2][0] * in[1][2])) *
        reciprocalDeterminant;
    out[1][1] =
        ((in[2][2] * in[0][0]) - (in[2][0] * in[0][2])) *
        reciprocalDeterminant;
    out[1][2] =
        -((in[1][2] * in[0][0]) - (in[1][0] * in[0][2])) *
        reciprocalDeterminant;
    out[2][0] =
        ((in[2][1] * in[1][0]) - (in[2][0] * in[1][1])) *
        reciprocalDeterminant;
    out[2][1] =
        -((in[2][1] * in[0][0]) - (in[2][0] * in[0][1])) *
        reciprocalDeterminant;
    out[2][2] =
        ((in[1][1] * in[0][0]) - (in[1][0] * in[0][1])) *
        reciprocalDeterminant;
#endif
}
