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

#include "core_math_private.h"

#if EMULATE_X87
#define CODUO_SK43_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_SK43_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 DObjSkelMatrixMultiply43(const DObjSkelMat *left,
                              const coduo_compact_affine_matrix43_t *right,
                              coduo_compact_affine_matrix43_t *out)
{
#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x080682e5): axis terms are
     * 80-bit three-term dots, origin terms add right->origin in the same chain,
     * all rounded to float. Column-major store order preserved. */
    for (int j = 0; j < 3; ++j) {
        out->axis[0][j] = CODUO_SK43_AXIS(left, 0, right, j);
        out->axis[1][j] = CODUO_SK43_AXIS(left, 1, right, j);
        out->axis[2][j] = CODUO_SK43_AXIS(left, 2, right, j);
    }
    for (int j = 0; j < 3; ++j) {
        out->origin[j] = CODUO_SK43_ORIGIN(left, right, j);
    }
#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->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];
#endif
}
