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

#include "core_math_private.h"

#if EMULATE_X87
#define CODUO_SK2_AXIS(l, i, r, j)                                             \
    x87f_store_f32(x87f_add(                                                   \
        x87f_add(x87f_mul(x87f_load_f32((l)->axis[i][0]),                     \
                          x87f_load_f32((r)->axis[0][j])),                    \
                 x87f_mul(x87f_load_f32((l)->axis[i][1]),                     \
                          x87f_load_f32((r)->axis[1][j]))),                   \
        x87f_mul(x87f_load_f32((l)->axis[i][2]),                             \
                 x87f_load_f32((r)->axis[2][j]))))
#define CODUO_SK2_ORIGIN(l, r, j)                                             \
    x87f_store_f32(x87f_add(                                                   \
        x87f_add(x87f_add(x87f_mul(x87f_load_f32((l)->origin[0]),            \
                                   x87f_load_f32((r)->axis[0][j])),          \
                          x87f_mul(x87f_load_f32((l)->origin[1]),            \
                                   x87f_load_f32((r)->axis[1][j]))),         \
                 x87f_mul(x87f_load_f32((l)->origin[2]),                     \
                          x87f_load_f32((r)->axis[2][j]))),                  \
        x87f_load_f32((r)->origin[j])))
#endif

void DObjSkel2MatrixMultiply43(const DObjSkelMat *left,
                               const coduo_compact_affine_matrix43_t *right,
                               DObjSkelMat *out)
{
#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x08068578): identical dot
     * structure to DObjSkelMatrixMultiply43 with an affine (padded) output; the
     * axis[i][3]=0 and origin[3]=1 stores are constants. */
    for (int j = 0; j < 3; ++j) {
        out->axis[0][j] = CODUO_SK2_AXIS(left, 0, right, j);
        out->axis[1][j] = CODUO_SK2_AXIS(left, 1, right, j);
        out->axis[2][j] = CODUO_SK2_AXIS(left, 2, right, j);
    }
    out->axis[0][3] = 0.0f;
    out->axis[1][3] = 0.0f;
    out->axis[2][3] = 0.0f;
    for (int j = 0; j < 3; ++j) {
        out->origin[j] = CODUO_SK2_ORIGIN(left, right, j);
    }
    out->origin[3] = 1.0f;
#else
    out->axis[0][0] = ((left->axis[0][0] * right->axis[0][0]) +
                       (left->axis[0][1] * right->axis[1][0])) +
                      (left->axis[0][2] * right->axis[2][0]);
    out->axis[1][0] = ((left->axis[1][0] * right->axis[0][0]) +
                       (left->axis[1][1] * right->axis[1][0])) +
                      (left->axis[1][2] * right->axis[2][0]);
    out->axis[2][0] = ((left->axis[2][0] * right->axis[0][0]) +
                       (left->axis[2][1] * right->axis[1][0])) +
                      (left->axis[2][2] * right->axis[2][0]);

    out->axis[0][1] = ((left->axis[0][0] * right->axis[0][1]) +
                       (left->axis[0][1] * right->axis[1][1])) +
                      (left->axis[0][2] * right->axis[2][1]);
    out->axis[1][1] = ((left->axis[1][0] * right->axis[0][1]) +
                       (left->axis[1][1] * right->axis[1][1])) +
                      (left->axis[1][2] * right->axis[2][1]);
    out->axis[2][1] = ((left->axis[2][0] * right->axis[0][1]) +
                       (left->axis[2][1] * right->axis[1][1])) +
                      (left->axis[2][2] * right->axis[2][1]);

    out->axis[0][2] = ((left->axis[0][0] * right->axis[0][2]) +
                       (left->axis[0][1] * right->axis[1][2])) +
                      (left->axis[0][2] * right->axis[2][2]);
    out->axis[1][2] = ((left->axis[1][0] * right->axis[0][2]) +
                       (left->axis[1][1] * right->axis[1][2])) +
                      (left->axis[1][2] * right->axis[2][2]);
    out->axis[2][2] = ((left->axis[2][0] * right->axis[0][2]) +
                       (left->axis[2][1] * right->axis[1][2])) +
                      (left->axis[2][2] * right->axis[2][2]);

    out->axis[0][3] = 0.0f;
    out->axis[1][3] = 0.0f;
    out->axis[2][3] = 0.0f;

    out->origin[0] = (((left->origin[0] * right->axis[0][0]) +
                       (left->origin[1] * right->axis[1][0])) +
                      (left->origin[2] * right->axis[2][0])) +
                     right->origin[0];
    out->origin[1] = (((left->origin[0] * right->axis[0][1]) +
                       (left->origin[1] * right->axis[1][1])) +
                      (left->origin[2] * right->axis[2][1])) +
                     right->origin[1];
    out->origin[2] = (((left->origin[0] * right->axis[0][2]) +
                       (left->origin[1] * right->axis[1][2])) +
                      (left->origin[2] * right->axis[2][2])) +
                     right->origin[2];
    out->origin[3] = 1.0f;
#endif
}
