#include <math.h>

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

#include "core_math_private.h"

long double VectorNormalize(vec3_t vector)
{
    float lengthSquared;
    float length;
    float inverseLength;

#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x08066755): dot product 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; 1.0/length in 80-bit (fld1; fdivs)
     * rounded to float; each component multiplied in 80-bit and rounded to
     * float. */
    x87f ls = x87f_mul(x87f_load_f32(vector[0]), x87f_load_f32(vector[0]));
    ls = x87f_add(ls, x87f_mul(x87f_load_f32(vector[1]),
                               x87f_load_f32(vector[1])));
    ls = x87f_add(ls, x87f_mul(x87f_load_f32(vector[2]),
                               x87f_load_f32(vector[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)));
        vector[0] = x87f_store_f32(
            x87f_mul(x87f_load_f32(vector[0]), x87f_load_f32(inverseLength)));
        vector[1] = x87f_store_f32(
            x87f_mul(x87f_load_f32(vector[1]), x87f_load_f32(inverseLength)));
        vector[2] = x87f_store_f32(
            x87f_mul(x87f_load_f32(vector[2]), x87f_load_f32(inverseLength)));
    }
#else
    lengthSquared = ((vector[0] * vector[0]) + (vector[1] * vector[1])) +
                    (vector[2] * vector[2]);
    length = (float)sqrt((double)lengthSquared);
    if (length != 0.0f) {
        inverseLength = 1.0f / length;
        vector[0] *= inverseLength;
        vector[1] *= inverseLength;
        vector[2] *= inverseLength;
    }
#endif

    return (long double)length;
}
