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

#include "core_math_private.h"

#include <string.h>

/* Each adjugate entry is a three-term 80-bit dot ((a*b + c*d) + e*f) then a
 * `-=` of a second such dot. The minors, column copies, and the final *=recip
 * scale are single-op (native-identical), so they stay plain in both builds;
 * only these multi-op chains go through the shim. Macro so the body (whose term
 * order mirrors the stock instruction stream, 0x8068c15..0x80690d6) is written
 * once. */
#if EMULATE_X87
#define CODUO_I4_DOT3(a, b, c, d, e, f)                                        \
    x87f_add(x87f_add(x87f_mul(x87f_load_f32(a), x87f_load_f32(b)),           \
                      x87f_mul(x87f_load_f32(c), x87f_load_f32(d))),          \
             x87f_mul(x87f_load_f32(e), x87f_load_f32(f)))
#define CODUO_I4_SET(dst, a, b, c, d, e, f)                                    \
    (dst) = x87f_store_f32(CODUO_I4_DOT3(a, b, c, d, e, f))
#define CODUO_I4_SUB(dst, a, b, c, d, e, f)                                    \
    (dst) = x87f_store_f32(                                                    \
        x87f_sub(x87f_load_f32(dst), CODUO_I4_DOT3(a, b, c, d, e, f)))
#else
#define CODUO_I4_SET(dst, a, b, c, d, e, f)                                    \
    (dst) = (((a) * (b)) + ((c) * (d))) + ((e) * (f))
#define CODUO_I4_SUB(dst, a, b, c, d, e, f)                                    \
    (dst) -= (((a) * (b)) + ((c) * (d))) + ((e) * (f))
#endif

void MatrixInverse44(const float in[4][4], float out[4][4])
{
    float column0[4];
    float column1[4];
    float column2[4];
    float column3[4];
    float minor0;
    float minor1;
    float minor2;
    float minor3;
    float minor4;
    float minor5;
    float minor6;
    float minor7;
    float minor8;
    float minor9;
    float minor10;
    float minor11;
    float reciprocalDeterminant;
    float *outValues;
    int i;

    for (i = 0; i < 4; i++) {
        memcpy(&column0[i], &in[i][0], sizeof(column0[i]));
        memcpy(&column1[i], &in[i][1], sizeof(column1[i]));
        memcpy(&column2[i], &in[i][2], sizeof(column2[i]));
        memcpy(&column3[i], &in[i][3], sizeof(column3[i]));
    }

    minor0 = column2[2] * column3[3];
    minor1 = column2[3] * column3[2];
    minor2 = column2[1] * column3[3];
    minor3 = column2[3] * column3[1];
    minor4 = column2[1] * column3[2];
    minor5 = column2[2] * column3[1];
    minor6 = column2[0] * column3[3];
    minor7 = column2[3] * column3[0];
    minor8 = column2[0] * column3[2];
    minor9 = column2[2] * column3[0];
    minor10 = column2[0] * column3[1];
    minor11 = column2[1] * column3[0];

    /* Term order in every adjugate sum below mirrors the stock instruction
     * stream (fld/fmul chains at 0x8068c15..0x80690d6); x87 addition order
     * matters, so do not reorder. */
    CODUO_I4_SET(out[0][0], minor0, column1[1], minor3, column1[2], minor4,
                 column1[3]);
    CODUO_I4_SUB(out[0][0], minor1, column1[1], minor2, column1[2], minor5,
                 column1[3]);
    CODUO_I4_SET(out[0][1], minor1, column1[0], minor6, column1[2], minor9,
                 column1[3]);
    CODUO_I4_SUB(out[0][1], minor0, column1[0], minor7, column1[2], minor8,
                 column1[3]);
    CODUO_I4_SET(out[0][2], minor2, column1[0], minor7, column1[1], minor10,
                 column1[3]);
    CODUO_I4_SUB(out[0][2], minor3, column1[0], minor6, column1[1], minor11,
                 column1[3]);
    CODUO_I4_SET(out[0][3], minor5, column1[0], minor8, column1[1], minor11,
                 column1[2]);
    CODUO_I4_SUB(out[0][3], minor4, column1[0], minor9, column1[1], minor10,
                 column1[2]);

    CODUO_I4_SET(out[1][0], minor1, column0[1], minor2, column0[2], minor5,
                 column0[3]);
    CODUO_I4_SUB(out[1][0], minor0, column0[1], minor3, column0[2], minor4,
                 column0[3]);
    CODUO_I4_SET(out[1][1], minor0, column0[0], minor7, column0[2], minor8,
                 column0[3]);
    CODUO_I4_SUB(out[1][1], minor1, column0[0], minor6, column0[2], minor9,
                 column0[3]);
    CODUO_I4_SET(out[1][2], minor3, column0[0], minor6, column0[1], minor11,
                 column0[3]);
    CODUO_I4_SUB(out[1][2], minor2, column0[0], minor7, column0[1], minor10,
                 column0[3]);
    CODUO_I4_SET(out[1][3], minor4, column0[0], minor9, column0[1], minor10,
                 column0[2]);
    CODUO_I4_SUB(out[1][3], minor5, column0[0], minor8, column0[1], minor11,
                 column0[2]);

    minor0 = column0[2] * column1[3];
    minor1 = column0[3] * column1[2];
    minor2 = column0[1] * column1[3];
    minor3 = column0[3] * column1[1];
    minor4 = column0[1] * column1[2];
    minor5 = column0[2] * column1[1];
    minor6 = column0[0] * column1[3];
    minor7 = column0[3] * column1[0];
    minor8 = column0[0] * column1[2];
    minor9 = column0[2] * column1[0];
    minor10 = column0[0] * column1[1];
    minor11 = column0[1] * column1[0];

    CODUO_I4_SET(out[2][0], minor0, column3[1], minor3, column3[2], minor4,
                 column3[3]);
    CODUO_I4_SUB(out[2][0], minor1, column3[1], minor2, column3[2], minor5,
                 column3[3]);
    CODUO_I4_SET(out[2][1], minor1, column3[0], minor6, column3[2], minor9,
                 column3[3]);
    CODUO_I4_SUB(out[2][1], minor0, column3[0], minor7, column3[2], minor8,
                 column3[3]);
    CODUO_I4_SET(out[2][2], minor2, column3[0], minor7, column3[1], minor10,
                 column3[3]);
    CODUO_I4_SUB(out[2][2], minor3, column3[0], minor6, column3[1], minor11,
                 column3[3]);
    CODUO_I4_SET(out[2][3], minor5, column3[0], minor8, column3[1], minor11,
                 column3[2]);
    CODUO_I4_SUB(out[2][3], minor4, column3[0], minor9, column3[1], minor10,
                 column3[2]);

    CODUO_I4_SET(out[3][0], minor2, column2[2], minor5, column2[3], minor1,
                 column2[1]);
    CODUO_I4_SUB(out[3][0], minor4, column2[3], minor0, column2[1], minor3,
                 column2[2]);
    CODUO_I4_SET(out[3][1], minor8, column2[3], minor0, column2[0], minor7,
                 column2[2]);
    CODUO_I4_SUB(out[3][1], minor6, column2[2], minor9, column2[3], minor1,
                 column2[0]);
    CODUO_I4_SET(out[3][2], minor6, column2[1], minor11, column2[3], minor3,
                 column2[0]);
    CODUO_I4_SUB(out[3][2], minor10, column2[3], minor2, column2[0], minor7,
                 column2[1]);
    CODUO_I4_SET(out[3][3], minor10, column2[2], minor4, column2[0], minor9,
                 column2[1]);
    CODUO_I4_SUB(out[3][3], minor8, column2[1], minor11, column2[2], minor5,
                 column2[0]);

    /* The stock binary rounds the determinant to a float slot before the
     * reciprocal divide (fstp/fld/fdiv at 0x8069107..0x806910c), reusing the
     * same slot for the result; keep the two-stage rounding. */
#if EMULATE_X87
    reciprocalDeterminant = x87f_store_f32(x87f_add(
        x87f_add(x87f_add(x87f_mul(x87f_load_f32(column0[0]),
                                   x87f_load_f32(out[0][0])),
                          x87f_mul(x87f_load_f32(column0[1]),
                                   x87f_load_f32(out[0][1]))),
                 x87f_mul(x87f_load_f32(column0[2]), x87f_load_f32(out[0][2]))),
        x87f_mul(x87f_load_f32(column0[3]), x87f_load_f32(out[0][3]))));
    reciprocalDeterminant = x87f_store_f32(
        x87f_div(x87f_load_f32(1.0f), x87f_load_f32(reciprocalDeterminant)));
#else
    reciprocalDeterminant =
        (column0[0] * out[0][0]) + (column0[1] * out[0][1]) +
        (column0[2] * out[0][2]) + (column0[3] * out[0][3]);
    reciprocalDeterminant = 1.0f / reciprocalDeterminant;
#endif

    outValues = &out[0][0];
    for (i = 0; i < 16; i++) {
        outValues[i] *= reciprocalDeterminant;
    }
}
