#include <math.h>

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

#include "core_math_private.h"

float VectorNormalize2(const vec3_t in, vec3_t out)
{
    float lengthSquared;
    float length;
    float inverseLength;

#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x080669a0), same op
     * structure as
     * VectorNormalize: dot accumulated in 80-bit and rounded to the float
     * lengthSquared slot (fstps); reloaded and stored as a double for the CRT
     * sqrt (fstpl; call sqrt) — sqrt is IEEE-mandated correctly-rounded, so
     * native sqrt(double) is bit-portable — result rounded to the float length;
     * the fldz/fucompp branch normalizes when length is nonzero OR NaN (C's
     * `length != 0.0f` is true for NaN too, matching); 1.0/length in 80-bit
     * (fld1; fdiv by the stored float length) rounded to float; each output
     * component multiplied in 80-bit and rounded to float. */
    x87f ls = x87f_mul(x87f_load_f32(in[0]), x87f_load_f32(in[0]));
    ls = x87f_add(ls, x87f_mul(x87f_load_f32(in[1]), x87f_load_f32(in[1])));
    ls = x87f_add(ls, x87f_mul(x87f_load_f32(in[2]), x87f_load_f32(in[2])));
    lengthSquared = x87f_store_f32(ls);
    length = (float)sqrt(x87f_store_f64(x87f_load_f32(lengthSquared)));
    if (length != 0.0f) {
        inverseLength = x87f_store_f32(
            x87f_div(x87f_load_f32(1.0f), x87f_load_f32(length)));
        out[0] = x87f_store_f32(
            x87f_mul(x87f_load_f32(in[0]), x87f_load_f32(inverseLength)));
        out[1] = x87f_store_f32(
            x87f_mul(x87f_load_f32(in[1]), x87f_load_f32(inverseLength)));
        out[2] = x87f_store_f32(
            x87f_mul(x87f_load_f32(in[2]), x87f_load_f32(inverseLength)));
    } else {
        out[2] = 0.0f;
        out[1] = 0.0f;
        out[0] = 0.0f;
    }
#else
    lengthSquared = ((in[0] * in[0]) + (in[1] * in[1])) + (in[2] * in[2]);
    length = (float)sqrt((double)lengthSquared);
    if (length != 0.0f) {
        inverseLength = 1.0f / length;
        out[0] = in[0] * inverseLength;
        out[1] = in[1] * inverseLength;
        out[2] = in[2] * inverseLength;
    } else {
        out[2] = 0.0f;
        out[1] = 0.0f;
        out[0] = 0.0f;
    }
#endif

    return length;
}
