#include <math.h>

#include "pxl8_math.h"
#include "pxl8_simd.h"

pxl8_vec2 pxl8_vec2_add(pxl8_vec2 a, pxl8_vec2 b) {
    return (pxl8_vec2) {
        .x = a.x + b.x,
        .y = a.y + b.y,
    };
}

pxl8_vec2 pxl8_vec2_sub(pxl8_vec2 a, pxl8_vec2 b) {
    return (pxl8_vec2) {
        .x = a.x - b.x,
        .y = a.y - b.y,
    };
}

pxl8_vec2 pxl8_vec2_scale(pxl8_vec2 v, f32 s) {
    return (pxl8_vec2) {
        .x = v.x * s,
        .y = v.y * s,
    };
}

f32 pxl8_vec2_dot(pxl8_vec2 a, pxl8_vec2 b) {
    return a.x * b.x + a.y * b.y;
}

f32 pxl8_vec2_length(pxl8_vec2 v) {
    return sqrtf(v.x * v.x + v.y * v.y);
}

pxl8_vec2 pxl8_vec2_normalize(pxl8_vec2 v) {
    f32 len = pxl8_vec2_length(v);
    if (len < 1e-6f) return (pxl8_vec2){0};
    return pxl8_vec2_scale(v, 1.0f / len);
}

pxl8_vec3 pxl8_vec3_add(pxl8_vec3 a, pxl8_vec3 b) {
    pxl8_simd_vec_f32 va = pxl8_simd_set_f32(a.x, a.y, a.z, 0);
    pxl8_simd_vec_f32 vb = pxl8_simd_set_f32(b.x, b.y, b.z, 0);
    pxl8_simd_vec_f32 result = pxl8_simd_add_f32(va, vb);

    return (pxl8_vec3) {
        .x = result.f32_array[0],
        .y = result.f32_array[1],
        .z = result.f32_array[2],
    };
}

pxl8_vec3 pxl8_vec3_sub(pxl8_vec3 a, pxl8_vec3 b) {
    pxl8_simd_vec_f32 va = pxl8_simd_set_f32(a.x, a.y, a.z, 0);
    pxl8_simd_vec_f32 vb = pxl8_simd_set_f32(b.x, b.y, b.z, 0);
    pxl8_simd_vec_f32 result = pxl8_simd_sub_f32(va, vb);

    return (pxl8_vec3) {
        .x = result.f32_array[0],
        .y = result.f32_array[1],
        .z = result.f32_array[2],
    };
}

pxl8_vec3 pxl8_vec3_scale(pxl8_vec3 v, f32 s) {
    pxl8_simd_vec_f32 vv = pxl8_simd_set_f32(v.x, v.y, v.z, 0);
    pxl8_simd_vec_f32 result = pxl8_simd_scale_f32(vv, s);

    return (pxl8_vec3) {
        .x = result.f32_array[0], 
        .y = result.f32_array[1], 
        .z = result.f32_array[2],
    };
}

f32 pxl8_vec3_dot(pxl8_vec3 a, pxl8_vec3 b) {
    pxl8_simd_vec_f32 va = pxl8_simd_set_f32(a.x, a.y, a.z, 0);
    pxl8_simd_vec_f32 vb = pxl8_simd_set_f32(b.x, b.y, b.z, 0);
    return pxl8_simd_dot3_f32(va, vb);
}

pxl8_vec3 pxl8_vec3_cross(pxl8_vec3 a, pxl8_vec3 b) {
    return (pxl8_vec3) {
        .x = a.y * b.z - a.z * b.y,
        .y = a.z * b.x - a.x * b.z,
        .z = a.x * b.y - a.y * b.x,
    };
}

f32 pxl8_vec3_length(pxl8_vec3 v) {
    return sqrtf(pxl8_vec3_dot(v, v));
}

pxl8_vec3 pxl8_vec3_normalize(pxl8_vec3 v) {
    f32 len = pxl8_vec3_length(v);
    if (len < 1e-6f) return (pxl8_vec3){0};
    return pxl8_vec3_scale(v, 1.0f / len);
}

pxl8_mat4 pxl8_mat4_identity(void) {
    pxl8_mat4 mat = {0};
    mat.m[0] = mat.m[5] = mat.m[10] = mat.m[15] = 1.0f;
    return mat;
}

pxl8_mat4 pxl8_mat4_multiply(pxl8_mat4 a, pxl8_mat4 b) {
    pxl8_mat4 result = {0};
    for (i32 i = 0; i < 4; i++) {
        for (i32 j = 0; j < 4; j++) {
            pxl8_simd_vec_f32 row = pxl8_simd_set_f32(
                a.m[i * 4 + 0], a.m[i * 4 + 1], a.m[i * 4 + 2], a.m[i * 4 + 3]
            );
            pxl8_simd_vec_f32 col = pxl8_simd_set_f32(
                b.m[0 * 4 + j], b.m[1 * 4 + j], b.m[2 * 4 + j], b.m[3 * 4 + j]
            );
            result.m[i * 4 + j] = pxl8_simd_dot4_f32(row, col);
        }
    }
    return result;
}

pxl8_vec4 pxl8_mat4_multiply_vec4(pxl8_mat4 m, pxl8_vec4 v) {
    pxl8_simd_vec_f32 vec = pxl8_simd_set_f32(v.x, v.y, v.z, v.w);
    pxl8_vec4 result;

    pxl8_simd_vec_f32 row0 = pxl8_simd_set_f32(m.m[0], m.m[1], m.m[2], m.m[3]);
    pxl8_simd_vec_f32 row1 = pxl8_simd_set_f32(m.m[4], m.m[5], m.m[6], m.m[7]);
    pxl8_simd_vec_f32 row2 = pxl8_simd_set_f32(m.m[8], m.m[9], m.m[10], m.m[11]);
    pxl8_simd_vec_f32 row3 = pxl8_simd_set_f32(m.m[12], m.m[13], m.m[14], m.m[15]);

    result.x = pxl8_simd_dot4_f32(row0, vec);
    result.y = pxl8_simd_dot4_f32(row1, vec);
    result.z = pxl8_simd_dot4_f32(row2, vec);
    result.w = pxl8_simd_dot4_f32(row3, vec);

    return result;
}

pxl8_mat4 pxl8_mat4_translate(f32 x, f32 y, f32 z) {
    pxl8_mat4 mat = pxl8_mat4_identity();
    mat.m[3] = x;
    mat.m[7] = y;
    mat.m[11] = z;
    return mat;
}

pxl8_mat4 pxl8_mat4_rotate_x(f32 angle) {
    pxl8_mat4 mat = pxl8_mat4_identity();
    f32 c = cosf(angle);
    f32 s = sinf(angle);
    mat.m[5] = c;
    mat.m[6] = -s;
    mat.m[9] = s;
    mat.m[10] = c;
    return mat;
}

pxl8_mat4 pxl8_mat4_rotate_y(f32 angle) {
    pxl8_mat4 mat = pxl8_mat4_identity();
    f32 c = cosf(angle);
    f32 s = sinf(angle);
    mat.m[0] = c;
    mat.m[2] = s;
    mat.m[8] = -s;
    mat.m[10] = c;
    return mat;
}

pxl8_mat4 pxl8_mat4_rotate_z(f32 angle) {
    pxl8_mat4 mat = pxl8_mat4_identity();
    f32 c = cosf(angle);
    f32 s = sinf(angle);
    mat.m[0] = c;
    mat.m[1] = -s;
    mat.m[4] = s;
    mat.m[5] = c;
    return mat;
}

pxl8_mat4 pxl8_mat4_scale(f32 x, f32 y, f32 z) {
    pxl8_mat4 mat = pxl8_mat4_identity();
    mat.m[0] = x;
    mat.m[5] = y;
    mat.m[10] = z;
    return mat;
}

pxl8_mat4 pxl8_mat4_ortho(f32 left, f32 right, f32 bottom, f32 top, f32 near, f32 far) {
    pxl8_mat4 mat = {0};

    mat.m[0] = 2.0f / (right - left);
    mat.m[5] = 2.0f / (top - bottom);
    mat.m[10] = -2.0f / (far - near);
    mat.m[3] = -(right + left) / (right - left);
    mat.m[7] = -(top + bottom) / (top - bottom);
    mat.m[11] = -(far + near) / (far - near);
    mat.m[15] = 1.0f;

    return mat;
}

pxl8_mat4 pxl8_mat4_perspective(f32 fov, f32 aspect, f32 near, f32 far) {
    pxl8_mat4 mat = {0};
    f32 tan_half_fov = tanf(fov / 2.0f);

    mat.m[0] = 1.0f / (aspect * tan_half_fov);
    mat.m[5] = 1.0f / tan_half_fov;
    mat.m[10] = -(far + near) / (far - near);
    mat.m[11] = -(2.0f * far * near) / (far - near);
    mat.m[14] = -1.0f;

    return mat;
}

pxl8_mat4 pxl8_mat4_lookat(pxl8_vec3 eye, pxl8_vec3 center, pxl8_vec3 up) {
    pxl8_vec3 f = pxl8_vec3_normalize(pxl8_vec3_sub(center, eye));
    pxl8_vec3 s = pxl8_vec3_normalize(pxl8_vec3_cross(f, up));
    pxl8_vec3 u = pxl8_vec3_cross(s, f);

    pxl8_mat4 mat = pxl8_mat4_identity();
    mat.m[0] = s.x;
    mat.m[1] = s.y;
    mat.m[2] = s.z;
    mat.m[4] = u.x;
    mat.m[5] = u.y;
    mat.m[6] = u.z;
    mat.m[8] = -f.x;
    mat.m[9] = -f.y;
    mat.m[10] = -f.z;
    mat.m[3] = -pxl8_vec3_dot(s, eye);
    mat.m[7] = -pxl8_vec3_dot(u, eye);
    mat.m[11] = pxl8_vec3_dot(f, eye);

    return mat;
}