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

#include "core_math_private.h"

#include <string.h>

#if !EMULATE_X87
/* NOT_FROM_ORIGINAL_SOURCE: volatile carrier for the stock 0xbf800000
 * multiplication operand used by AngleVectors. Modern GCC otherwise folds
 * `-1.0f * value` into FCHS even at -O0, which changes signaling-NaN behavior
 * from the original FMUL graph. */
static const volatile float engine_compat_angle_vectors_negative_one = -1.0f;
#endif

void AngleVectors(const vec3_t angles, vec3_t forward, vec3_t right, vec3_t up)
{
    float angle;
    float sr;
    float sp;
    float sy;
    float cr;
    float cp;
    float cy;

#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x080674ad): each angle is
     * angles[i]*(pi/180) in 80-bit rounded to the float passed to SinCosFloat;
     * each forward/right/up component is a whole-expression 80-bit product/sum
     * rounded to float at the store. SinCosFloat itself is the fsincos leaf —
     * on the x86 differential host both builds use the same fsincos instruction,
     * so this envelope is verifiable; the arm64 fsincos is the separate deferred
     * problem. */
    angle = x87f_store_f32(x87f_mul(x87f_load_f32(angles[1]),
                                    x87f_load_f64(3.141592653589793 / 180.0)));
    SinCosFloat(angle, &sy, &cy);

    angle = x87f_store_f32(x87f_mul(x87f_load_f32(angles[0]),
                                    x87f_load_f64(3.141592653589793 / 180.0)));
    SinCosFloat(angle, &sp, &cp);

    if (forward != NULL) {
        uint32_t bits;

        forward[0] =
            x87f_store_f32(x87f_mul(x87f_load_f32(cp), x87f_load_f32(cy)));
        forward[1] =
            x87f_store_f32(x87f_mul(x87f_load_f32(cp), x87f_load_f32(sy)));
        memcpy(&bits, &sp, sizeof(bits));
        bits ^= CODUO_FLOAT_SIGN_BIT;
        memcpy(&forward[2], &bits, sizeof(bits));
    }

    if ((right != NULL) || (up != NULL)) {
        angle = x87f_store_f32(x87f_mul(
            x87f_load_f32(angles[2]), x87f_load_f64(3.141592653589793 / 180.0)));
        SinCosFloat(angle, &sr, &cr);

        if (right != NULL) {
            /* (((-1*sr)*sp)*cy) + ((-1*cr)*(-sy)) */
            right[0] = x87f_store_f32(x87f_add(
                x87f_mul(
                    x87f_mul(
                        x87f_mul(x87f_load_f32(-1.0f),
                                 x87f_load_f32(sr)),
                        x87f_load_f32(sp)),
                    x87f_load_f32(cy)),
                x87f_mul(
                    x87f_mul(x87f_load_f32(-1.0f),
                             x87f_load_f32(cr)),
                    x87f_neg(x87f_load_f32(sy)))));
            /* (((-1*sr)*sp)*sy) + ((-1*cr)*cy) */
            right[1] = x87f_store_f32(x87f_add(
                x87f_mul(
                    x87f_mul(
                        x87f_mul(x87f_load_f32(-1.0f),
                                 x87f_load_f32(sr)),
                        x87f_load_f32(sp)),
                    x87f_load_f32(sy)),
                x87f_mul(
                    x87f_mul(x87f_load_f32(-1.0f),
                             x87f_load_f32(cr)),
                    x87f_load_f32(cy))));
            right[2] = x87f_store_f32(x87f_mul(
                x87f_mul(x87f_load_f32(-1.0f), x87f_load_f32(sr)),
                x87f_load_f32(cp)));
        }

        if (up != NULL) {
            /* ((cr*sp)*cy) + (sr*sy) */
            up[0] = x87f_store_f32(x87f_add(
                x87f_mul(x87f_mul(x87f_load_f32(cr), x87f_load_f32(sp)),
                         x87f_load_f32(cy)),
                x87f_mul(x87f_load_f32(sr), x87f_load_f32(sy))));
            /* ((cr*sp)*sy) + (-sr*cy) */
            up[1] = x87f_store_f32(x87f_add(
                x87f_mul(x87f_mul(x87f_load_f32(cr), x87f_load_f32(sp)),
                         x87f_load_f32(sy)),
                x87f_mul(x87f_neg(x87f_load_f32(sr)), x87f_load_f32(cy))));
            up[2] =
                x87f_store_f32(x87f_mul(x87f_load_f32(cr), x87f_load_f32(cp)));
        }
    }
#else
    angle = (float)((double)angles[1] * (3.141592653589793 / 180.0));
    SinCosFloat(angle, &sy, &cy);

    angle = (float)((double)angles[0] * (3.141592653589793 / 180.0));
    SinCosFloat(angle, &sp, &cp);

    if (forward != NULL) {
        uint32_t bits;

        forward[0] = cp * cy;
        forward[1] = cp * sy;
        memcpy(&bits, &sp, sizeof(bits));
        bits ^= CODUO_FLOAT_SIGN_BIT;
        memcpy(&forward[2], &bits, sizeof(bits));
    }

    if ((right != NULL) || (up != NULL)) {
        angle = (float)((double)angles[2] * (3.141592653589793 / 180.0));
        SinCosFloat(angle, &sr, &cr);

        if (right != NULL) {
            /* Preserve the stock FMUL/FCHS/FADD graph at
             * 0x0806757d..0x080675e7. */
            right[0] =
                engine_compat_angle_vectors_negative_one * sr * sp * cy +
                engine_compat_angle_vectors_negative_one * cr * -sy;
            right[1] =
                engine_compat_angle_vectors_negative_one * sr * sp * sy +
                engine_compat_angle_vectors_negative_one * cr * cy;
            right[2] = engine_compat_angle_vectors_negative_one * sr * cp;
        }

        if (up != NULL) {
            up[0] = (cr * sp * cy) + (sr * sy);
            up[1] = (cr * sp * sy) + (-sr * cy);
            up[2] = cr * cp;
        }
    }
#endif
}
