#include <math.h>

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

#include "core_math_private.h"

long double VectorNormalize4(vec4_t vector)
{
    float lengthSquared;
    float length;
    float inverseLength;

#if EMULATE_X87
    /* x87-faithful transcription (stock coduo_lnxded 0x0806686a): the four squared
     * terms are summed left-to-right in 80-bit and lengthSquared rounded to a float
     * slot before sqrt (fstps), reloaded/widened to double for the correctly-rounded
     * CRT sqrt, then the reciprocal and four component scales computed in 80-bit and
     * rounded to float. */
    x87f ls =
        x87f_add(x87f_mul(x87f_load_f32(vector[0]), x87f_load_f32(vector[0])),
                 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])));
    ls = x87f_add(ls,
                  x87f_mul(x87f_load_f32(vector[3]), x87f_load_f32(vector[3])));
    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)));
        vector[3] = x87f_store_f32(
            x87f_mul(x87f_load_f32(vector[3]), x87f_load_f32(inverseLength)));
    }
#else
    lengthSquared = (((vector[0] * vector[0]) + (vector[1] * vector[1])) +
                     (vector[2] * vector[2])) +
                    (vector[3] * vector[3]);
    length = (float)sqrt((double)lengthSquared);
    if (length != 0.0f) {
        inverseLength = 1.0f / length;
        vector[0] = vector[0] * inverseLength;
        vector[1] = vector[1] * inverseLength;
        vector[2] = vector[2] * inverseLength;
        vector[3] = vector[3] * inverseLength;
    }
#endif

    return (long double)length;
}
