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

v2 v2a(f32 x)
{
	return (v2){x, x};
}

v3 v3a(f32 x)
{
	return (v3){x, x, x};
}

v2 scale_v2(v2 v, f32 x)
{
	return (v2){v.x * x, v.y * x};
}

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

v2 addf_v2(v2 a, f32 b)
{
	return (v2){a.x + b, a.y + b};
}

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

v2 clamp_v2(v2 v, v2 min, v2 max)
{
	f32 x = v.x < min.x ? min.x : v.x > max.x ? max.x : v.x;
	f32 y = v.y < min.y ? min.y : v.y > max.y ? max.y : v.y;
	return (v2){x, y};
}

f32 length_v2(v2 v)
{
	return sqrtf(v.x * v.x + v.y * v.y);
}

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

v2 norm_v2(v2 a)
{
	f32 len = length_v2(a);
	return (v2){a.x / len, a.y / len};
}

direction_enum direction_v2(v2 v)
{
	v2 directions[] = {
		{1.0f, 0.0f},
		{0.0f, 1.0f},
		{-1.0f, 0.0f},
		{0.0f, -1.0f}
	};
	f32 max = 0.0f;
	i32 best = 0;
	for (i32 i = 0; i < 4; i++) {
		f32 dot = dot_v2(v, directions[i]);
		if (dot > max) {
			max = dot;
			best = i;
		}
	}
	return best;
}

mat mul_mat(mat l, mat r)
{
	f32 l00 = l.c0.x, l01 = l.c0.y, l02 = l.c0.z, l03 = l.c0.w;
	f32 l10 = l.c1.x, l11 = l.c1.y, l12 = l.c1.z, l13 = l.c1.w;
	f32 l20 = l.c2.x, l21 = l.c2.y, l22 = l.c2.z, l23 = l.c2.w;
	f32 l30 = l.c3.x, l31 = l.c3.y, l32 = l.c3.z, l33 = l.c3.w;

	f32 r00 = r.c0.x, r01 = r.c0.y, r02 = r.c0.z, r03 = r.c0.w;
	f32 r10 = r.c1.x, r11 = r.c1.y, r12 = r.c1.z, r13 = r.c1.w;
	f32 r20 = r.c2.x, r21 = r.c2.y, r22 = r.c2.z, r23 = r.c2.w;
	f32 r30 = r.c3.x, r31 = r.c3.y, r32 = r.c3.z, r33 = r.c3.w;

	mat result = {
		{
			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
		}
	};

	return result;
}

mat make_scale_mat(v3 v)
{
	mat m = {0};
	m.c0.x = v.x;
	m.c1.y = v.y;
	m.c2.z = v.z;
	m.c3.w = 1.0f;
	return m;
}

mat make_rotate_mat(v3 x, v3 y, v3 z)
{
	mat m = {
		{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}
	};
	return m;
}

mat make_translate_mat(v3 v)
{
	mat m = mat_identity;
	m.c3.x = v.x;
	m.c3.y = v.y;
	m.c3.z = v.z;
	return m;
}

mat scale_mat(mat m, v3 v)
{
	return mul_mat(make_scale_mat(v), m);
}

mat rotate_mat(mat m, 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);
 
	v3 x = {cy * cz, sx * sy * cz + cx * sz, -cx * sy * cz + sx * sz};
	v3 y = {-cy * sz, -sx * sy * sz + cx * cz, cx * sy * sz + sx * cz};
	v3 z = {sy, -sx * cy, cx * cy};

	return mul_mat(make_rotate_mat(x, y, z), m);
}

mat translate_mat(mat m, v3 v)
{
	return mul_mat(make_translate_mat(v), m);
}

mat ortho(f32 l, f32 r, f32 b, f32 t, f32 n, f32 f)
{
	mat ortho = {0};
	ortho.c0.x =  2.0f / (r - l); 
	ortho.c1.y =  2.0f / (t - b);
	ortho.c2.z = -2.0f / (f - n);
	ortho.c3.x = -((r + l) / (r - l));
	ortho.c3.y = -((t + b) / (t - b));
	ortho.c3.z = -((f + n) / (f - n));
	ortho.c3.w =  1.0f;
	return ortho;
}