#ifndef _Q_MATRIX_H
#define _Q_MATRIX_H

/* Linear algebra - vectors and matrices: */

typedef float vec_t;
typedef vec_t vec2_t[2];
typedef vec_t vec3_t[3];
typedef vec_t vec4_t[4];
typedef vec_t vec5_t[5];

typedef struct {
    union {
	vec_t   i[3][3];
	vec_t	array[9];
    };
} mat3_t;

typedef struct {
    union {
	vec_t   i[4][4];
	vec_t	flat[16];
    };
} mat4_t;

#define DotProduct(x,y)         ((x)[0]*(y)[0]+(x)[1]*(y)[1]+(x)[2]*(y)[2])

#define CrossProduct(v1,v2,cross) \
        ((cross)[0]=(v1)[1]*(v2)[2]-(v1)[2]*(v2)[1], \
         (cross)[1]=(v1)[2]*(v2)[0]-(v1)[0]*(v2)[2], \
         (cross)[2]=(v1)[0]*(v2)[1]-(v1)[1]*(v2)[0])

#define VectorSubtract(a,b,c) \
        ((c)[0]=(a)[0]-(b)[0], \
         (c)[1]=(a)[1]-(b)[1], \
         (c)[2]=(a)[2]-(b)[2])

#define VectorAdd(a,b,c) \
        ((c)[0]=(a)[0]+(b)[0], \
         (c)[1]=(a)[1]+(b)[1], \
         (c)[2]=(a)[2]+(b)[2])

#define VectorAdd3(a,b,c,d) \
        ((d)[0]=(a)[0]+(b)[0]+(c)[0], \
         (d)[1]=(a)[1]+(b)[1]+(c)[1], \
         (d)[2]=(a)[2]+(b)[2]+(c)[2])

#define VectorCopy(a,b)     ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
#define VectorClear(a)      ((a)[0]=(a)[1]=(a)[2]=0)
#define VectorNegate(a,b)   ((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2])
#define VectorInverse(a)    ((a)[0]=-(a)[0],(a)[1]=-(a)[1],(a)[2]=-(a)[2])
#define VectorSet(v, x, y, z)   ((v)[0]=(x),(v)[1]=(y),(v)[2]=(z))
#define VectorAvg(a,b,c) \
        ((c)[0]=((a)[0]+(b)[0])*0.5f, \
         (c)[1]=((a)[1]+(b)[1])*0.5f, \
         (c)[2]=((a)[2]+(b)[2])*0.5f)
#define VectorMA(a,b,c,d) \
        ((d)[0]=(a)[0]+(b)*(c)[0], \
         (d)[1]=(a)[1]+(b)*(c)[1], \
         (d)[2]=(a)[2]+(b)*(c)[2])
#define VectorVectorMA(a,b,c,d) \
        ((d)[0]=(a)[0]+(b)[0]*(c)[0], \
         (d)[1]=(a)[1]+(b)[1]*(c)[1], \
         (d)[2]=(a)[2]+(b)[2]*(c)[2])
#define VectorEmpty(v) ((v)[0]==0&&(v)[1]==0&&(v)[2]==0)
#define VectorCompare(v1,v2)    ((v1)[0]==(v2)[0]&&(v1)[1]==(v2)[1]&&(v1)[2]==(v2)[2])
#define VectorLength(v)		    sqrt(DotProduct((v),(v)))
#define VectorLengthSquared(v)      DotProduct((v),(v))

#define VectorScale(in,scale,out) \
        ((out)[0]=(in)[0]*(scale), \
         (out)[1]=(in)[1]*(scale), \
         (out)[2]=(in)[2]*(scale))
#define VectorVectorScale(in,scale,out) \
        ((out)[0]=(in)[0]*(scale)[0], \
         (out)[1]=(in)[1]*(scale)[1], \
         (out)[2]=(in)[2]*(scale)[2])

#define VectorScaleAcc(in,scale,out) \
        ((out)[0]+=(in)[0]*(scale), \
         (out)[1]+=(in)[1]*(scale), \
         (out)[2]+=(in)[2]*(scale))
#define VectorVectorScaleAcc(in,scale,out) \
        ((out)[0]+=(in)[0]*(scale)[0], \
         (out)[1]+=(in)[1]*(scale)[1], \
         (out)[2]+=(in)[2]*(scale)[2])

#define DistanceSquared(v1,v2) \
        (((v1)[0]-(v2)[0])*((v1)[0]-(v2)[0])+ \
        ((v1)[1]-(v2)[1])*((v1)[1]-(v2)[1])+ \
        ((v1)[2]-(v2)[2])*((v1)[2]-(v2)[2]))
#define Distance(v1,v2) (sqrt(DistanceSquared(v1,v2)))
#define LerpAngles(a,b,c,d) \
        ((d)[0]=LerpAngle((a)[0],(b)[0],c), \
         (d)[1]=LerpAngle((a)[1],(b)[1],c), \
         (d)[2]=LerpAngle((a)[2],(b)[2],c))

#define LerpVector(a,b,c,d) \
    ((d)[0]=(a)[0]+(c)*((b)[0]-(a)[0]), \
     (d)[1]=(a)[1]+(c)*((b)[1]-(a)[1]), \
     (d)[2]=(a)[2]+(c)*((b)[2]-(a)[2]))

#define LerpVector2(a,b,c,d,e)		    \
    (VectorClear(e),			    \
     VectorScaleAcc(a, c, e),		    \
     VectorScaleAcc(b, d, e))

#define PlaneDiff(v,p)   (DotProduct(v,(p)->normal)-(p)->dist)

#define Vector4Subtract(a,b,c)  ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2],(c)[3]=(a)[3]-(b)[3])
#define Vector4Add(a,b,c)       ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2],(c)[3]=(a)[3]+(b)[3])
#define Vector4Copy(a,b)        ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
#define Vector4Clear(a)         ((a)[0]=(a)[1]=(a)[2]=(a)[3]=0)
#define Vector4Negate(a,b)      ((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2],(b)[3]=-(a)[3])
#define Vector4Set(v, a, b, c, d)   ((v)[0]=(a),(v)[1]=(b),(v)[2]=(c),(v)[3]=(d))

void AngleVectors(vec3_t angles, vec3_t forward, vec3_t right, vec3_t up);

void VectorNormalize(vec3_t v);
void VectorNormalize2(vec3_t v, vec3_t out);
vec_t VectorNormalizeLen(vec3_t v);        // returns vector length
mat4_t mul_mat4(const mat4_t *restrict a, const mat4_t *restrict b);

static inline void AnglesToAxis(vec3_t angles, vec3_t axis[3])
{
    AngleVectors(angles, axis[0], axis[1], axis[2]);
    VectorInverse(axis[1]);
}

static inline void TransposeAxis(vec3_t axis[3])
{
    vec_t temp;

    temp = axis[0][1];
    axis[0][1] = axis[1][0];
    axis[1][0] = temp;

    temp = axis[0][2];
    axis[0][2] = axis[2][0];
    axis[2][0] = temp;

    temp = axis[1][2];
    axis[1][2] = axis[2][1];
    axis[2][1] = temp;
}

static inline void RotatePoint(vec3_t point, vec3_t axis[3])
{
    vec3_t temp;

    VectorCopy(point, temp);
    point[0] = DotProduct(temp, axis[0]);
    point[1] = DotProduct(temp, axis[1]);
    point[2] = DotProduct(temp, axis[2]);
}

static inline mat3_t vec3_to_mat3(vec3_t v0, vec3_t v1, vec3_t v2)
{
    return (mat3_t) {
	.i = {
	    [0][0] = v0[0],
	    [0][1] = v0[1],
	    [0][2] = v0[2],

	    [1][0] = v1[0],
	    [1][1] = v1[1],
	    [1][2] = v1[2],

	    [2][0] = v2[0],
	    [2][1] = v2[1],
	    [2][2] = v2[2],
	},
    };
}

static inline mat4_t AffineMatrix(mat3_t m, vec3_t v)
{
    mat4_t a;

    for (unsigned i = 0; i < 3; i++) {
	for (unsigned j = 0; j < 3; j++)
	    a.i[i][j] = m.i[i][j];

	a.i[3][i] = v[i];
    }

    a.i[0][3] = 0;
    a.i[1][3] = 0;
    a.i[2][3] = 0;
    a.i[3][3] = 1;

    return a;
}

static const mat3_t mat3_identity = {
    .i = {
	[0][0] = 1,
	[1][1] = 1,
	[2][2] = 1,
    }
};

#endif /* _Q_MATRIX_H */