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

#include <string.h>

#include "core_math_private.h"

#if EMULATE_X87
/* one row-by-column 80-bit dot ((a*p + b*q) + c*r) rounded to float. */
#define CODUO_MME_DOT(la, lb, lc, ra, rb, rc)                                  \
    x87f_store_f32(x87f_add(                                                   \
        x87f_add(x87f_mul(x87f_load_f32(la), x87f_load_f32(ra)),              \
                 x87f_mul(x87f_load_f32(lb), x87f_load_f32(rb))),             \
        x87f_mul(x87f_load_f32(lc), x87f_load_f32(rc))))
#endif

void MatrixMultiplyEquals(const vec3_t left[3], vec3_t right[3])
{
#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x08067b08): each result is
     * an 80-bit three-term dot rounded to float. In-place order preserved — rows
     * 0/1 go through temps, row 2 is written directly, all reading the original
     * `right` before the temps are stored back. */
    const float right00 = CODUO_MME_DOT(left[0][0], left[0][1], left[0][2],
                                        right[0][0], right[1][0], right[2][0]);
    const float right01 = CODUO_MME_DOT(left[0][0], left[0][1], left[0][2],
                                        right[0][1], right[1][1], right[2][1]);
    const float right02 = CODUO_MME_DOT(left[0][0], left[0][1], left[0][2],
                                        right[0][2], right[1][2], right[2][2]);
    const float right10 = CODUO_MME_DOT(left[1][0], left[1][1], left[1][2],
                                        right[0][0], right[1][0], right[2][0]);
    const float right11 = CODUO_MME_DOT(left[1][0], left[1][1], left[1][2],
                                        right[0][1], right[1][1], right[2][1]);
    const float right12 = CODUO_MME_DOT(left[1][0], left[1][1], left[1][2],
                                        right[0][2], right[1][2], right[2][2]);

    right[2][0] = CODUO_MME_DOT(left[2][0], left[2][1], left[2][2], right[0][0],
                                right[1][0], right[2][0]);
    right[2][1] = CODUO_MME_DOT(left[2][0], left[2][1], left[2][2], right[0][1],
                                right[1][1], right[2][1]);
    right[2][2] = CODUO_MME_DOT(left[2][0], left[2][1], left[2][2], right[0][2],
                                right[1][2], right[2][2]);
#else
    const float right00 = (left[0][0] * right[0][0]) +
                          (left[0][1] * right[1][0]) +
                          (left[0][2] * right[2][0]);
    const float right01 = (left[0][0] * right[0][1]) +
                          (left[0][1] * right[1][1]) +
                          (left[0][2] * right[2][1]);
    const float right02 = (left[0][0] * right[0][2]) +
                          (left[0][1] * right[1][2]) +
                          (left[0][2] * right[2][2]);
    const float right10 = (left[1][0] * right[0][0]) +
                          (left[1][1] * right[1][0]) +
                          (left[1][2] * right[2][0]);
    const float right11 = (left[1][0] * right[0][1]) +
                          (left[1][1] * right[1][1]) +
                          (left[1][2] * right[2][1]);
    const float right12 = (left[1][0] * right[0][2]) +
                          (left[1][1] * right[1][2]) +
                          (left[1][2] * right[2][2]);

    right[2][0] = (left[2][0] * right[0][0]) +
                  (left[2][1] * right[1][0]) +
                  (left[2][2] * right[2][0]);
    right[2][1] = (left[2][0] * right[0][1]) +
                  (left[2][1] * right[1][1]) +
                  (left[2][2] * right[2][1]);
    right[2][2] = (left[2][0] * right[0][2]) +
                  (left[2][1] * right[1][2]) +
                  (left[2][2] * right[2][2]);
#endif

    /* Stock copies the six saved results back as raw dwords, in this order. */
    memcpy(&right[0][0], &right00, sizeof(right[0][0]));
    memcpy(&right[0][1], &right01, sizeof(right[0][1]));
    memcpy(&right[0][2], &right02, sizeof(right[0][2]));
    memcpy(&right[1][0], &right10, sizeof(right[1][0]));
    memcpy(&right[1][1], &right11, sizeof(right[1][1]));
    memcpy(&right[1][2], &right12, sizeof(right[1][2]));
}
