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

#include "core_math_private.h"

void MatrixInverseTransformVector43(
    const vec3_t in, const coduo_compact_affine_matrix43_t *matrix, vec3_t out)
{
    const float translated0 = in[0] - matrix->origin[0];
    const float translated1 = in[1] - matrix->origin[1];
    const float translated2 = in[2] - matrix->origin[2];

#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x08069416): the translate
     * subtractions are single-op (native-identical); each output is the 80-bit
     * dot ((a0*t0 + a1*t1) + a2*t2) rounded to float at the store. */
    for (int i = 0; i < 3; ++i) {
        out[i] = x87f_store_f32(x87f_add(
            x87f_add(
                x87f_mul(x87f_load_f32(matrix->axis[i][0]),
                         x87f_load_f32(translated0)),
                x87f_mul(x87f_load_f32(matrix->axis[i][1]),
                         x87f_load_f32(translated1))),
            x87f_mul(x87f_load_f32(matrix->axis[i][2]),
                     x87f_load_f32(translated2))));
    }
#else
    out[0] = (matrix->axis[0][0] * translated0 +
              matrix->axis[0][1] * translated1) +
             matrix->axis[0][2] * translated2;
    out[1] = (matrix->axis[1][0] * translated0 +
              matrix->axis[1][1] * translated1) +
             matrix->axis[1][2] * translated2;
    out[2] = (matrix->axis[2][0] * translated0 +
              matrix->axis[2][1] * translated1) +
             matrix->axis[2][2] * translated2;
#endif
}
