#include "prbm.h"
#include <math.h>

struct v2 fillv2(f32 a)
{
	return (struct v2){a, a};
}

struct v2 invv2(struct v2 a)
{
	return (struct v2){-a.x, -a.y};
}

struct v2 addv2(struct v2 a, struct v2 b)
{
	return (struct v2){a.x + b.x, a.y + b.y};
}

struct v2 subv2(struct v2 a, struct v2 b)
{
	return (struct v2){a.x - b.x, a.y - b.y};
}

struct v2 scalev2(struct v2 a, f32 s)
{
	return (struct v2){a.x * s, a.y * s};
}

struct v2 scalevv2(struct v2 a, struct v2 s)
{
	return (struct v2){a.x * s.x, a.y * s.y};
}

f32 dotv2(struct v2 a, struct v2 b)
{
	return (a.x * b.x + a.y * b.y);
}

f32 len2v2(struct v2 a)
{
	return dotv2(a, a);
}

f32 lenv2(struct v2 a)
{
	return sqrtf(len2v2(a));
}

struct v2 normv2(struct v2 a)
{
	f32 len = lenv2(a);
	if (len) {
		return v2_zero;
	}
	f32 ilen = 1.0f / len;
	return (struct v2){a.x * len, a.y * len};
}

struct v3 fillv3(f32 a)
{
	return (struct v3){a, a, a};
}

struct v3 invv3(struct v3 a)
{
	return (struct v3){-a.x, -a.y, -a.z};
}

struct v3 addv3(struct v3 a, struct v3 b)
{
	return (struct v3){a.x + b.x, a.y + b.y, a.z + b.z};
}

struct v3 subv3(struct v3 a, struct v3 b)
{
	return (struct v3){a.x - b.x, a.y - b.y, a.z - b.z};
}

struct v3 scalev3(struct v3 a, f32 s)
{
	return (struct v3){a.x * s, a.y * s, a.z * s};
}

struct v3 scalevv3(struct v3 a, struct v3 s)
{
	return (struct v3){a.x * s.x, a.y * s.y, a.z * s.z};
}

f32 dotv3(struct v3 a, struct v3 b)
{
	return (a.x * b.x + a.y * b.y + a.z * b.z);
}

struct v3 crossv3(struct v3 l, struct v3 r)
{
	return (struct v3){
		(l.y * r.z - r.y * l.z),
		(r.x * l.z - l.x * r.z),
		(l.x * r.y - r.x * l.y)
	};
}

f32 len2v3(struct v3 a)
{
	return dotv3(a, a);
}

f32 lenv3(struct v3 a)
{
	return sqrtf(len2v3(a));
}

struct v3 normv3(struct v3 a)
{
	f32 len = lenv3(a);
	if (!len)
		return v3_zero;
	f32 ilen = 1.0f / len;
	return (struct v3){a.x * len, a.y * len, a.z * len};
}

struct v4 fillv4(f32 a)
{
	return (struct v4){a, a, a, a};
}

struct v4 invv4(struct v4 a)
{
	return (struct v4){-a.x, -a.y, -a.z, -a.w};
}

struct v4 addv4(struct v4 a, struct v4 b)
{
	return (struct v4){a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w};
}

struct v4 subv4(struct v4 a, struct v4 b)
{
	return (struct v4){a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w};
}

struct v4 scalev4(struct v4 a, f32 s)
{
	return (struct v4){a.x * s, a.y * s, a.z * s, a.w * s};
}

struct v4 scalevv4(struct v4 a, struct v4 s)
{
	return (struct v4){a.x * s.x, a.y * s.y, a.z * s.z, a.w * s.w};
}

f32 dotv4(struct v4 a, struct v4 b)
{
	return (a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}

f32 len2v4(struct v4 a)
{
	return dotv4(a, a);
}

f32 lenv4(struct v4 a)
{
	return sqrtf(len2v4(a));
}

struct v4 normv4(struct v4 a)
{
	f32 len = lenv4(a);
	if (!len)
		return v4_zero;
	f32 ilen = 1.0f / len;
	return (struct v4){a.x * ilen, a.y * ilen, a.z * ilen, a.w * ilen};
}

struct mat transpmat(struct mat m)
{
	struct mat r = m;
	r.c0.y = m.c1.x;
	r.c0.z = m.c2.x;
	r.c0.w = m.c3.x;
	r.c1.z = m.c2.y;
	r.c1.w = m.c3.y;
	r.c2.w = m.c3.z;
	return r;
}

struct mat mulmat(struct mat left, struct mat right)
{
	f32 l00 = left.c0.x, l01 = left.c0.y, l02 = left.c0.z, l03 = left.c0.w;
	f32 l10 = left.c1.x, l11 = left.c1.y, l12 = left.c1.z, l13 = left.c1.w;
	f32 l20 = left.c2.x, l21 = left.c2.y, l22 = left.c2.z, l23 = left.c2.w;
	f32 l30 = left.c3.x, l31 = left.c3.y, l32 = left.c3.z, l33 = left.c3.w;
	f32 r00 = right.c0.x, r01 = right.c0.y, r02 = right.c0.z, r03 = right.c0.w;
	f32 r10 = right.c1.x, r11 = right.c1.y, r12 = right.c1.z, r13 = right.c1.w;
	f32 r20 = right.c2.x, r21 = right.c2.y, r22 = right.c2.z, r23 = right.c2.w;
	f32 r30 = right.c3.x, r31 = right.c3.y, r32 = right.c3.z, r33 = right.c3.w;
	return (struct mat){
	{
		l00*r00 + l10*r01 + l20*r02 + l30*r03,
		l01*r00 + l11*r01 + l21*r02 + l31*r03,
		l02*r00 + l12*r01 + l22*r02 + l32*r03,
		l03*r00 + l13*r01 + l23*r02 + l33*r03
	},
	{
		l00*r10 + l10*r11 + l20*r12 + l30*r13,
		l01*r10 + l11*r11 + l21*r12 + l31*r13,
		l02*r10 + l12*r11 + l22*r12 + l32*r13,
		l03*r10 + l13*r11 + l23*r12 + l33*r13
	},
	{
		l00*r20 + l10*r21 + l20*r22 + l30*r23,
		l01*r20 + l11*r21 + l21*r22 + l31*r23,
		l02*r20 + l12*r21 + l22*r22 + l32*r23,
		l03*r20 + l13*r21 + l23*r22 + l33*r23
	},
	{
		l00*r30 + l10*r31 + l20*r32 + l30*r33,
		l01*r30 + l11*r31 + l21*r32 + l31*r33,
		l02*r30 + l12*r31 + l22*r32 + l32*r33,
		l03*r30 + l13*r31 + l23*r32 + l33*r33
	}
	};
}

struct mat make_translate(struct v3 v)
{
	return (struct mat){
		{1.0f, 0.0f, 0.0f, 0.0f},
		{0.0f, 1.0f, 0.0f, 0.0f},
		{0.0f, 0.0f, 1.0f, 0.0f},
		{v.x,  v.y,  v.z,  1.0f}
	};
}

struct mat make_scale(struct v3 v)
{
	return (struct mat){
		{v.x,  0.0f, 0.0f, 0.0f},
		{0.0f, v.y,  0.0f, 0.0f},
		{0.0f, 0.0f, v.z,  0.0f},
		{0.0f, 0.0f, 0.0f, 1.0f}
	};
}

struct mat make_rotate(struct v3 x, struct v3 y, struct v3 z)
{
	return (struct mat){
		{x.x,  x.y,  x.z,  0.0f},
		{y.x,  y.y,  y.z,  0.0f},
		{z.x,  z.y,  z.z,  0.0f},
		{0.0f, 0.0f, 0.0f, 1.0f}
	};
}

struct mat translate_mat(struct mat m, struct v3 v)
{
	return mulmat(make_translate(v), m);
}

struct mat scale_mat(struct mat m, struct v3 v)
{
	return mulmat(make_scale(v), m);
}

struct mat rotate_mat(struct mat m, struct v3 angles)
{
	f32 angle = deg2rad(angles.x);
	f32 cx = cosf(angle);
	f32 sx = sinf(angle);
	angle = deg2rad(angles.y);
	f32 cy = cosf(angle);
	f32 sy = sinf(angle);
	angle = deg2rad(angles.z);
	f32 cz = cosf(angle);
	f32 sz = sinf(angle);
	struct v3 x = { cy*cz,  sx*sy*cz + cx*sz, -cx*sy*cz + sx*sz};
	struct v3 y = {-cy*sz, -sx*sy*sz + cx*cz,  cx*sy*sz + sx*cz};
	struct v3 z = { sy,    -sx*cy,             cx*cy};
	return mulmat(make_rotate(x, y, z), m);
}

struct v4 mulmatv4(struct mat m, struct v4 v)
{
	return (struct v4){
		m.c0.x*v.x + m.c1.x*v.y + m.c2.x*v.z + m.c3.x*v.w,
		m.c0.y*v.x + m.c1.y*v.y + m.c2.y*v.z + m.c3.y*v.w,
		m.c0.z*v.x + m.c1.z*v.y + m.c2.z*v.z + m.c3.z*v.w,
		m.c0.w*v.x + m.c1.w*v.y + m.c2.w*v.z + m.c3.w*v.w
	};
}

i32 in_rect(struct v2 pos, struct rect rect)
{
	return (pos.x > rect.start.x) && (pos.x < rect.end.x) &&
		(pos.y > rect.start.y) && (pos.y < rect.end.y);
}