#include <math.h>

#include "shared/shared.h"

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

    angle = DEG2RAD(angles[YAW]);
    sy = sin(angle);
    cy = cos(angle);
    angle = DEG2RAD(angles[PITCH]);
    sp = sin(angle);
    cp = cos(angle);
    angle = DEG2RAD(angles[ROLL]);
    sr = sin(angle);
    cr = cos(angle);

    if (forward) {
        forward[0] = cp * cy;
        forward[1] = cp * sy;
        forward[2] = -sp;
    }
    if (right) {
        right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
        right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
        right[2] = -1 * sr * cp;
    }
    if (up) {
        up[0] = (cr * sp * cy + -sr * -sy);
        up[1] = (cr * sp * sy + -sr * cy);
        up[2] = cr * cp;
    }
}

static float inv_sqrt(float n)
{
	float x2 = n * 0.5F;
	union {
	    float   f;
	    int	    i;
	} y;

	/* magic number for doubles: 0x5fe6eb50c7b537a9 */

	y.f = n;
	y.i = 0x5f375a86 - (y.i >> 1);
	y.f = y.f * (1.5F - (x2 * y.f * y.f));
	y.f = y.f * (1.5F - (x2 * y.f * y.f));

	return y.f;
}

void VectorNormalize(vec3_t v)
{
    VectorScale(v, inv_sqrt(VectorLengthSquared(v)), v);
}

void VectorNormalize2(vec3_t v, vec3_t out)
{
    VectorScale(v, inv_sqrt(VectorLengthSquared(v)), out);
}

vec_t VectorNormalizeLen(vec3_t v)
{
    float length = VectorLength(v);

    if (length)
	VectorScale(v, 1 / length, v);

    return length;
}

mat4_t mul_mat4(const mat4_t *restrict a, const mat4_t *restrict b)
{
    mat4_t p = { 0 };

    for (int i = 0; i < 4; i++)
        for (int j = 0; j < 4; j++)
	    for (int k = 0; k < 4; k++)
		p.i[i][j] += a->i[k][j] * b->i[i][k];

    return p;
}