#include <string.h>

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

#include "core_math_private.h"

void RotatePointAroundVector(vec3_t dst, const vec3_t dir,
                             const vec3_t point, float degrees)
{
    vec3_t forward;
    vec3_t right;
    vec3_t up;
    float matrix[3][3];
    float inverse[3][3];
    float zrot[3][3];
    float tmp[3][3];
    float rot[3][3];
    int i;

    memcpy(&forward[0], &dir[0], sizeof(forward[0]));
    memcpy(&forward[1], &dir[1], sizeof(forward[1]));
    memcpy(&forward[2], &dir[2], sizeof(forward[2]));

    PerpendicularVector(right, dir);
    CrossProduct(right, forward, up);

    memcpy(&matrix[0][0], &right[0], sizeof(matrix[0][0]));
    memcpy(&matrix[1][0], &right[1], sizeof(matrix[1][0]));
    memcpy(&matrix[2][0], &right[2], sizeof(matrix[2][0]));
    memcpy(&matrix[0][1], &up[0], sizeof(matrix[0][1]));
    memcpy(&matrix[1][1], &up[1], sizeof(matrix[1][1]));
    memcpy(&matrix[2][1], &up[2], sizeof(matrix[2][1]));
    memcpy(&matrix[0][2], &forward[0], sizeof(matrix[0][2]));
    memcpy(&matrix[1][2], &forward[1], sizeof(matrix[1][2]));
    memcpy(&matrix[2][2], &forward[2], sizeof(matrix[2][2]));

    memcpy(inverse, matrix, sizeof(inverse));
    memcpy(&inverse[0][1], &matrix[1][0], sizeof(inverse[0][1]));
    memcpy(&inverse[0][2], &matrix[2][0], sizeof(inverse[0][2]));
    memcpy(&inverse[1][0], &matrix[0][1], sizeof(inverse[1][0]));
    memcpy(&inverse[1][2], &matrix[2][1], sizeof(inverse[1][2]));
    memcpy(&inverse[2][0], &matrix[0][2], sizeof(inverse[2][0]));
    memcpy(&inverse[2][1], &matrix[1][2], sizeof(inverse[2][1]));

    memset(zrot, 0, sizeof(zrot));
    zrot[2][2] = 1.0f;
    zrot[1][1] = 1.0f;
    zrot[0][0] = 1.0f;

#if EMULATE_X87
    /* stock forms (degrees*pi)/180 in 80-bit (multiply then divide, not the
     * folded pi/180 constant) before the fsincos leaf. */
    SinCosFloat(
        x87f_store_f32(x87f_div(x87f_mul(x87f_load_f32(degrees),
                                         x87f_load_f64(3.141592653589793)),
                                x87f_load_f64(180.0))),
        &zrot[0][1], &zrot[0][0]);
#else
    SinCosFloat((float)(((double)degrees * 3.141592653589793) / 180.0),
                 &zrot[0][1], &zrot[0][0]);
#endif
    {
        uint32_t sineBits;

        memcpy(&sineBits, &zrot[0][1], sizeof(sineBits));
        sineBits ^= CODUO_FLOAT_SIGN_BIT;
        memcpy(&zrot[1][0], &sineBits, sizeof(zrot[1][0]));
    }
    memcpy(&zrot[1][1], &zrot[0][0], sizeof(zrot[1][1]));

    MatrixMultiply(matrix, zrot, tmp);
    MatrixMultiply(tmp, inverse, rot);

    for (i = 0; i < 3; i++) {
#if EMULATE_X87
        /* 80-bit dot ((rot0*p0 + rot1*p1) + rot2*p2) rounded to float. */
        dst[i] = x87f_store_f32(x87f_add(
            x87f_add(x87f_mul(x87f_load_f32(rot[i][0]), x87f_load_f32(point[0])),
                     x87f_mul(x87f_load_f32(rot[i][1]), x87f_load_f32(point[1]))),
            x87f_mul(x87f_load_f32(rot[i][2]), x87f_load_f32(point[2]))));
#else
        dst[i] = (rot[i][0] * point[0]) + (rot[i][1] * point[1]) +
                 (rot[i][2] * point[2]);
#endif
    }
}
