#ifndef MATH_LINALG_H
#define MATH_LINALG_H

/*
 * Minimal 3D math: Vec3 and column-major Mat4 (OpenGL layout).
 *
 * Note on rotations: mat4_rotate_x/y/z rotate clockwise when looking down
 * the positive axis (i.e. by -angle in the right-handed convention).
 * Render, collision and player code all rely on this, so keep it consistent.
 */

#include <math.h>

#define PI_F 3.14159265358979323846f

typedef struct {
    float x, y, z;
} Vec3;

typedef struct {
    float m[16];
} Mat4;

static inline float deg2rad(float deg) { return deg * (PI_F / 180.0f); }

static inline float clampf(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }
static inline float lerpf(float a, float b, float t) { return a + (b - a) * t; }

static inline float smoothstepf(float edge0, float edge1, float x) {
    float t = clampf((x - edge0) / (edge1 - edge0), 0.0f, 1.0f);
    return t * t * (3.0f - 2.0f * t);
}

/* ---------- Vec3 ---------- */

static inline Vec3 vec3(float x, float y, float z) { return (Vec3){ x, y, z }; }
static inline Vec3 vec3_zero(void) { return (Vec3){ 0.0f, 0.0f, 0.0f }; }

static inline Vec3 vec3_add(Vec3 a, Vec3 b) { return (Vec3){ a.x + b.x, a.y + b.y, a.z + b.z }; }
static inline Vec3 vec3_sub(Vec3 a, Vec3 b) { return (Vec3){ a.x - b.x, a.y - b.y, a.z - b.z }; }
static inline Vec3 vec3_scale(Vec3 v, float s) { return (Vec3){ v.x * s, v.y * s, v.z * s }; }
static inline Vec3 vec3_lerp(Vec3 a, Vec3 b, float t) {
    return (Vec3){ lerpf(a.x, b.x, t), lerpf(a.y, b.y, t), lerpf(a.z, b.z, t) };
}

static inline float vec3_dot(Vec3 a, Vec3 b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
static inline float vec3_length(Vec3 v) { return sqrtf(vec3_dot(v, v)); }

static inline Vec3 vec3_cross(Vec3 a, Vec3 b) {
    return (Vec3){
        a.y * b.z - a.z * b.y,
        a.z * b.x - a.x * b.z,
        a.x * b.y - a.y * b.x,
    };
}

/* Returns the zero vector for (near) zero-length input instead of NaN. */
static inline Vec3 vec3_normalize(Vec3 v) {
    float len = vec3_length(v);
    if (len < 1e-8f) return vec3_zero();
    return vec3_scale(v, 1.0f / len);
}

/* ---------- Mat4 ---------- */

static inline Mat4 mat4_identity(void) {
    Mat4 m = { { 0 } };
    m.m[0] = m.m[5] = m.m[10] = m.m[15] = 1.0f;
    return m;
}

static inline Mat4 mat4_mul(Mat4 a, Mat4 b) {
    Mat4 r = { { 0 } };
    for (int col = 0; col < 4; ++col)
        for (int row = 0; row < 4; ++row)
            for (int k = 0; k < 4; ++k)
                r.m[row + col * 4] += a.m[row + k * 4] * b.m[k + col * 4];
    return r;
}

/* Transforms a direction (w = 0), ignoring translation. */
static inline Vec3 mat4_mul_dir(Mat4 m, Vec3 v) {
    return (Vec3){
        m.m[0] * v.x + m.m[4] * v.y + m.m[8]  * v.z,
        m.m[1] * v.x + m.m[5] * v.y + m.m[9]  * v.z,
        m.m[2] * v.x + m.m[6] * v.y + m.m[10] * v.z,
    };
}

/* Inverse rotation of a direction (transpose of the upper 3x3). */
static inline Vec3 mat4_mul_dir_transposed(Mat4 m, Vec3 v) {
    return (Vec3){
        m.m[0] * v.x + m.m[1] * v.y + m.m[2]  * v.z,
        m.m[4] * v.x + m.m[5] * v.y + m.m[6]  * v.z,
        m.m[8] * v.x + m.m[9] * v.y + m.m[10] * v.z,
    };
}

/* Transforms a point (w = 1). Assumes an affine matrix. */
static inline Vec3 mat4_mul_point(Mat4 m, Vec3 p) {
    Vec3 r = mat4_mul_dir(m, p);
    return (Vec3){ r.x + m.m[12], r.y + m.m[13], r.z + m.m[14] };
}

static inline Mat4 mat4_scale(Vec3 v) {
    Mat4 m = mat4_identity();
    m.m[0] = v.x;
    m.m[5] = v.y;
    m.m[10] = v.z;
    return m;
}

static inline Mat4 mat4_translate(Vec3 v) {
    Mat4 m = mat4_identity();
    m.m[12] = v.x;
    m.m[13] = v.y;
    m.m[14] = v.z;
    return m;
}

static inline Mat4 mat4_rotate_x(float rad) {
    Mat4 m = mat4_identity();
    float c = cosf(rad), s = sinf(rad);
    m.m[5] = c;  m.m[6]  = -s;
    m.m[9] = s;  m.m[10] = c;
    return m;
}

static inline Mat4 mat4_rotate_y(float rad) {
    Mat4 m = mat4_identity();
    float c = cosf(rad), s = sinf(rad);
    m.m[0] = c;  m.m[2]  = s;
    m.m[8] = -s; m.m[10] = c;
    return m;
}

static inline Mat4 mat4_rotate_z(float rad) {
    Mat4 m = mat4_identity();
    float c = cosf(rad), s = sinf(rad);
    m.m[0] = c;  m.m[1] = -s;
    m.m[4] = s;  m.m[5] = c;
    return m;
}

/* Euler rotation applied as Y (yaw), then X, then Z. Used everywhere. */
static inline Mat4 mat4_rotate_yxz(Vec3 rad) {
    Mat4 r = mat4_rotate_y(rad.y);
    r = mat4_mul(r, mat4_rotate_x(rad.x));
    return mat4_mul(r, mat4_rotate_z(rad.z));
}

/*
 * Yaw (rotation.y) conventions, the one place to look them up. A yaw turns
 * local +X to (cos, 0, sin) and local +Z to (-sin, 0, cos).
 *   yaw_along(dir):  the yaw that lays local +X along dir (lengths: logs, gates, walls)
 *   yaw_facing(dir): the yaw that turns local +Z (an object's front: furniture,
 *                    doors, windows) towards dir
 *   yaw_front(yaw):  where local +Z points after that yaw
 */
static inline float yaw_along(Vec3 dir)  { return atan2f(dir.z, dir.x); }
static inline float yaw_facing(Vec3 dir) { return atan2f(-dir.x, dir.z); }
static inline Vec3  yaw_front(float yaw) { return vec3(-sinf(yaw), 0.0f, cosf(yaw)); }

/*
 * Inverse of mat4_rotate_yxz: Euler angles of the rotation that turns the
 * local Y axis into y_axis and the local Z axis into z_axis (both unit and
 * perpendicular to each other).
 */
static inline Vec3 euler_yxz_from_axes(Vec3 y_axis, Vec3 z_axis) {
    Vec3 x_axis = vec3_cross(y_axis, z_axis);
    float sx = z_axis.y < -1.0f ? -1.0f : (z_axis.y > 1.0f ? 1.0f : z_axis.y);
    if (fabsf(sx) > 0.9999f) {
        /* Z straight up or down (something lying flat, like a log): yaw and roll turn
           about the same axis, so all of it goes into the roll */
        float roll = sx > 0.0f ? atan2f(y_axis.x, -y_axis.z) : atan2f(y_axis.x, y_axis.z);
        return vec3(sx > 0.0f ? PI_F * 0.5f : -PI_F * 0.5f, 0.0f, roll);
    }
    return vec3(asinf(sx), -atan2f(z_axis.x, z_axis.z), -atan2f(x_axis.y, y_axis.y));
}

static inline Mat4 mat4_perspective(float fov_deg, float aspect, float near_z, float far_z) {
    float f = 1.0f / tanf(deg2rad(fov_deg) * 0.5f);
    Mat4 m = { { 0 } };
    m.m[0]  = f / aspect;
    m.m[5]  = f;
    m.m[10] = -(far_z + near_z) / (far_z - near_z);
    m.m[11] = -1.0f;
    m.m[14] = -(2.0f * far_z * near_z) / (far_z - near_z);
    return m;
}

static inline Mat4 mat4_orthographic(float left, float right, float bottom, float top,
                                     float near_z, float far_z) {
    Mat4 m = { { 0 } };
    m.m[0]  = 2.0f / (right - left);
    m.m[5]  = 2.0f / (top - bottom);
    m.m[10] = -2.0f / (far_z - near_z);
    m.m[12] = -(right + left) / (right - left);
    m.m[13] = -(top + bottom) / (top - bottom);
    m.m[14] = -(far_z + near_z) / (far_z - near_z);
    m.m[15] = 1.0f;
    return m;
}

static inline Mat4 mat4_look_at(Vec3 eye, Vec3 center, Vec3 up) {
    Vec3 f = vec3_normalize(vec3_sub(center, eye));
    Vec3 s = vec3_normalize(vec3_cross(f, up));
    Vec3 u = vec3_cross(s, f);

    Mat4 m = mat4_identity();
    m.m[0] = s.x;  m.m[4] = s.y;  m.m[8]  = s.z;
    m.m[1] = u.x;  m.m[5] = u.y;  m.m[9]  = u.z;
    m.m[2] = -f.x; m.m[6] = -f.y; m.m[10] = -f.z;
    m.m[12] = -vec3_dot(s, eye);
    m.m[13] = -vec3_dot(u, eye);
    m.m[14] =  vec3_dot(f, eye);
    return m;
}

#endif /* MATH_LINALG_H */