#include "maths.h"

#include <math.h>

vec2_t vec2_add(vec2_t v1, vec2_t v2)
{
	return (vec2_t) { v1.x + v2.x, v1.y + v2.y };
}

vec2_t vec2_scale(vec2_t v, float s)
{
	return (vec2_t) { s * v.x, s * v.y };
}

vec3_t vec2_extend(vec2_t v)
{
	return (vec3_t) { v.x, v.y, 1 };
}

mat2_t mat2_rotation(float theta)
{
	return (mat2_t) {
		{ cosf(theta), sinf(theta) },
		{ -sinf(theta), cosf(theta) },
	};
}

vec2_t mat2_mul_vec2(mat2_t m, vec2_t v)
{
	return (vec2_t) {
		m.x.x * v.x + m.y.x * v.y,
		m.x.y * v.x + m.y.y * v.y,
	};
}

mat2_t mat2_mul_mat2(mat2_t m1, mat2_t m2)
{
#define ELEM(i, j) m1.x.j *m2.i.x + m1.y.j *m2.i.y
	return (mat2_t) {
		{ ELEM(x, x), ELEM(x, y) },
		{ ELEM(y, x), ELEM(y, y) },
	};
#undef ELEM
}

mat3_t mat2_extend(mat2_t m)
{
	return (mat3_t) {
		{ m.x.x, m.x.y, 0 },
		{ m.y.x, m.y.y, 0 },
		{ 0, 0, 1 },
	};
}

mat3_t mat3_translation(vec2_t v)
{
	return (mat3_t) {
		{ 1, 0, 0 },
		{ 0, 1, 0 },
		{ v.x, v.y, 1 },
	};
}

vec3_t mat3_mul_vec3(mat3_t m, vec3_t v)
{
	return (vec3_t) {
		m.x.x * v.x + m.y.x * v.y + m.z.x * v.z,
		m.x.y * v.x + m.y.y * v.y + m.z.y * v.z,
		m.x.z * v.x + m.y.z * v.y + m.z.z * v.z,
	};
}

mat3_t mat3_mul_mat3(mat3_t m1, mat3_t m2)
{
#define ELEM(i, j) m1.x.j *m2.i.x + m1.y.j *m2.i.y + m1.z.j *m2.i.z
	return (mat3_t) {
		{ ELEM(x, x), ELEM(x, y), ELEM(x, z) },
		{ ELEM(y, x), ELEM(y, y), ELEM(y, z) },
		{ ELEM(z, x), ELEM(z, y), ELEM(z, z) },
	};
#undef ELEM
}