// This file is part of the 64k demo project.
// It provides shared vector, matrix and animation utilities, templatized and
// inlined.

#pragma once
#include <cmath>

// --- Configuration ---
#define USE_VEC2
#define USE_VEC3
#define USE_VEC4
#define USE_QUAT
#define USE_MAT4
#define USE_EASING
#define USE_SPRING

// --- Operator Macro ---
// T: Class Name (e.g., vec3)
// N: Number of active components for math (e.g., 3)
#define VEC_OPERATORS(T, N)                                                    \
  float& operator[](int i) {                                                   \
    return v[i];                                                               \
  }                                                                            \
  const float& operator[](int i) const {                                       \
    return v[i];                                                               \
  }                                                                            \
  T& operator+=(const T& r) {                                                  \
    for (int i = 0; i < N; ++i)                                                \
      v[i] += r.v[i];                                                          \
    return *this;                                                              \
  }                                                                            \
  T& operator-=(const T& r) {                                                  \
    for (int i = 0; i < N; ++i)                                                \
      v[i] -= r.v[i];                                                          \
    return *this;                                                              \
  }                                                                            \
  T& operator*=(float s) {                                                     \
    for (int i = 0; i < N; ++i)                                                \
      v[i] *= s;                                                               \
    return *this;                                                              \
  }                                                                            \
  T operator+(const T& r) const {                                              \
    T res(*this);                                                              \
    res += r;                                                                  \
    return res;                                                                \
  }                                                                            \
  T operator-(const T& r) const {                                              \
    T res(*this);                                                              \
    res -= r;                                                                  \
    return res;                                                                \
  }                                                                            \
  T operator*(float s) const {                                                 \
    T res(*this);                                                              \
    res *= s;                                                                  \
    return res;                                                                \
  }                                                                            \
  T operator-() const {                                                        \
    T res;                                                                     \
    for (int i = 0; i < N; ++i)                                                \
      res.v[i] = -v[i];                                                        \
    return res;                                                                \
  }                                                                            \
  static float dot(const T& a, const T& b) {                                   \
    float s = 0;                                                               \
    for (int i = 0; i < N; ++i)                                                \
      s += a.v[i] * b.v[i];                                                    \
    return s;                                                                  \
  }                                                                            \
  float dot(const T& a) const {                                                \
    return dot(*this, a);                                                      \
  }                                                                            \
  float norm() const {                                                         \
    return std::sqrt(dot(*this, *this));                                       \
  }                                                                            \
  float len() const {                                                          \
    return norm();                                                             \
  }                                                                            \
  float inv_norm() const {                                                     \
    float l2 = dot(*this, *this);                                              \
    return l2 > 0 ? 1.0f / std::sqrt(l2) : 0;                                  \
  }                                                                            \
  T normalize() const {                                                        \
    return (*this) * inv_norm();                                               \
  }

#if defined(USE_VEC2)
struct vec2 {
  union {
    struct {
      float x, y;
    };
    float v[2];
  };
  vec2(float x = 0, float y = 0) : x(x), y(y) {
  }
  VEC_OPERATORS(vec2, 2)
};
#endif /* defined(USE_VEC2) */

#if defined(USE_VEC3)
struct vec3 {
  union {
    struct {
      float x, y, z;
      float _;
    }; // _ is padding for 16-byte alignment
    float v[4]; // Size 4 to match alignment
  };
  vec3(float x = 0, float y = 0, float z = 0) : x(x), y(y), z(z), _(0) {
  }
  VEC_OPERATORS(vec3, 3) // Operators only touch x,y,z (indices 0,1,2)

  static vec3 cross(vec3 a, vec3 b) {
    return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z,
            a.x * b.y - a.y * b.x};
  }
};
#endif /* defined(USE_VEC3) */

#if defined(USE_VEC4)
struct vec4 {
  union {
    struct {
      float x, y, z, w;
    };
    float v[4];
  };
  vec4(float x = 0, float y = 0, float z = 0, float w = 0)
      : x(x), y(y), z(z), w(w) {
  }
  VEC_OPERATORS(vec4, 4)
};
#endif /* defined(USE_VEC4) */

#if defined(USE_MAT4)
struct mat4 {
  float m[16] = {1, 0, 0, 0, 0, 1, 0, 0,
                 0, 0, 1, 0, 0, 0, 0, 1}; // Identity (Column-Major)

  // Array access
  float& operator[](int i) {
    return m[i];
  }
  const float& operator[](int i) const {
    return m[i];
  }

  // Matrix multiplication
  mat4 operator*(const mat4& r) const {
    mat4 res;
    for (int col = 0; col < 4; ++col) {
      for (int row = 0; row < 4; ++row) {
        float sum = 0.0f;
        for (int k = 0; k < 4; ++k) {
          sum += m[k * 4 + row] * r.m[col * 4 + k];
        }
        res.m[col * 4 + row] = sum;
      }
    }
    return res;
  }

  // Vector multiplication (Transform)
  vec4 operator*(const vec4& v) const {
    vec4 res;
    for (int row = 0; row < 4; ++row) {
      res.v[row] = m[row] * v.x + m[row + 4] * v.y + m[row + 8] * v.z +
                   m[row + 12] * v.w;
    }
    return res;
  }

  // Translation
  static mat4 translate(vec3 t) {
    mat4 r; // Identity
    r.m[12] = t.x;
    r.m[13] = t.y;
    r.m[14] = t.z;
    return r;
  }

  // Scaling
  static mat4 scale(vec3 s) {
    mat4 r; // Identity
    r.m[0] = s.x;
    r.m[5] = s.y;
    r.m[10] = s.z;
    return r;
  }

  // Rotation (Axis-Angle)
  static mat4 rotate(vec3 axis, float angle) {
    vec3 a = axis.normalize();
    float s = std::sin(angle);
    float c = std::cos(angle);
    float oc = 1.0f - c;

    mat4 r;
    r.m[0] = oc * a.x * a.x + c;
    r.m[1] = oc * a.x * a.y + a.z * s;
    r.m[2] = oc * a.x * a.z - a.y * s;

    r.m[4] = oc * a.x * a.y - a.z * s;
    r.m[5] = oc * a.y * a.y + c;
    r.m[6] = oc * a.y * a.z + a.x * s;

    r.m[8] = oc * a.x * a.z + a.y * s;
    r.m[9] = oc * a.y * a.z - a.x * s;
    r.m[10] = oc * a.z * a.z + c;
    return r;
  }

  static mat4 perspective(float fov, float asp, float n, float f) {
    mat4 r = {};
    float t = 1.0f / std::tan(fov * 0.5f);
    r.m[0] = t / asp;
    r.m[5] = t;
    r.m[10] = f / (n - f);
    r.m[11] = -1;
    r.m[14] = (n * f) / (n - f);
    return r;
  }

  static mat4 look_at(vec3 eye, vec3 center, vec3 up) {
    vec3 f = (center - eye).normalize();
    vec3 s = vec3::cross(f, up).normalize();
    vec3 u = vec3::cross(s, f);
    mat4 res;
    res.m[0] = s.x;
    res.m[4] = s.y;
    res.m[8] = s.z;
    res.m[1] = u.x;
    res.m[5] = u.y;
    res.m[9] = u.z;
    res.m[2] = -f.x;
    res.m[6] = -f.y;
    res.m[10] = -f.z;
    res.m[12] = -vec3::dot(s, eye);
    res.m[13] = -vec3::dot(u, eye);
    res.m[14] = vec3::dot(f, eye);
    return res;
  }

  // --- New Methods ---
  
  static mat4 transpose(const mat4& in) {
    mat4 out;
    for (int col = 0; col < 4; ++col) {
      for (int row = 0; row < 4; ++row) {
        out.m[row * 4 + col] = in.m[col * 4 + row];
      }
    }
    return out;
  }

  mat4 inverse() const {
    mat4 inv;
    inv[0] = m[5]  * m[10] * m[15] - m[5]  * m[11] * m[14] - m[9]  * m[6]  * m[15] + m[9]  * m[7]  * m[14] + m[13] * m[6]  * m[11] - m[13] * m[7]  * m[10];
    inv[4] = -m[4]  * m[10] * m[15] + m[4]  * m[11] * m[14] + m[8]  * m[6]  * m[15] - m[8]  * m[7]  * m[14] - m[12] * m[6]  * m[11] + m[12] * m[7]  * m[10];
    inv[8] = m[4]  * m[9] * m[15] - m[4]  * m[11] * m[13] - m[8]  * m[5]  * m[15] + m[8]  * m[7]  * m[13] + m[12] * m[5]  * m[11] - m[12] * m[7]  * m[9];
    inv[12] = -m[4]  * m[9] * m[14] + m[4]  * m[10] * m[13] + m[8]  * m[5]  * m[14] - m[8]  * m[6]  * m[13] - m[12] * m[5]  * m[10] + m[12] * m[6]  * m[9];
    inv[1] = -m[1]  * m[10] * m[15] + m[1]  * m[11] * m[14] + m[9]  * m[2]  * m[15] - m[9]  * m[3]  * m[14] - m[13] * m[2]  * m[11] + m[13] * m[3]  * m[10];
    inv[5] = m[0]  * m[10] * m[15] - m[0]  * m[11] * m[14] - m[8]  * m[2]  * m[15] + m[8]  * m[3]  * m[14] + m[12] * m[2]  * m[11] - m[12] * m[3]  * m[10];
    inv[9] = -m[0]  * m[9] * m[15] + m[0]  * m[11] * m[13] + m[8]  * m[1]  * m[15] - m[8]  * m[3]  * m[13] - m[12] * m[1]  * m[11] + m[12] * m[3]  * m[9];
    inv[13] = m[0]  * m[9] * m[14] - m[0]  * m[10] * m[13] - m[8]  * m[1]  * m[14] + m[8]  * m[2]  * m[13] + m[12] * m[1]  * m[10] - m[12] * m[2]  * m[9];
    inv[2] = m[1]  * m[6] * m[15] - m[1]  * m[7] * m[14] - m[5]  * m[2] * m[15] + m[5]  * m[3] * m[14] + m[13] * m[2] * m[7] - m[13] * m[3] * m[6];
    inv[6] = -m[0]  * m[6] * m[15] + m[0]  * m[7] * m[14] + m[4]  * m[2] * m[15] - m[4]  * m[3] * m[14] - m[12] * m[2] * m[7] + m[12] * m[3] * m[6];
    inv[10] = m[0]  * m[5] * m[15] - m[0]  * m[7] * m[13] - m[4]  * m[1] * m[15] + m[4]  * m[3] * m[13] + m[12] * m[1] * m[7] - m[12] * m[3] * m[5];
    inv[14] = -m[0]  * m[5] * m[14] + m[0]  * m[6] * m[13] + m[4]  * m[1] * m[14] - m[4]  * m[2] * m[13] - m[12] * m[1] * m[6] + m[12] * m[2] * m[5];
    inv[3] = -m[1] * m[6] * m[11] + m[1] * m[7] * m[10] + m[5] * m[2] * m[11] - m[5] * m[3] * m[10] - m[9] * m[2] * m[7] + m[9] * m[3] * m[6];
    inv[7] = m[0] * m[6] * m[11] - m[0] * m[7] * m[10] - m[4] * m[2] * m[11] + m[4] * m[3] * m[10] + m[8] * m[2] * m[7] - m[8] * m[3] * m[6];
    inv[11] = -m[0] * m[5] * m[11] + m[0] * m[7] * m[9] + m[4] * m[1] * m[11] - m[4] * m[3] * m[9] - m[8] * m[1] * m[7] + m[8] * m[3] * m[5];
    inv[15] = m[0] * m[5] * m[10] - m[0] * m[6] * m[9] - m[4] * m[1] * m[10] + m[4] * m[2] * m[9] + m[8] * m[1] * m[6] - m[8] * m[2] * m[5];

    float det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12];
    if (det == 0) return mat4(); // Return identity on failure

    det = 1.0f / det;
    for (int i = 0; i < 16; i++) inv[i] = inv[i] * det;
    return inv;
  }
};
#endif /* defined(USE_MAT4) */

#if defined(USE_QUAT)
struct quat {
  union {
    struct {
      float x, y, z, w;
    };
    float v[4];
  };
  quat(float x = 0, float y = 0, float z = 0, float w = 1)
      : x(x), y(y), z(z), w(w) {
  }
  VEC_OPERATORS(quat, 4)

  quat operator*(const quat& q) const {
    return {w * q.x + x * q.w + y * q.z - z * q.y,
            w * q.y - x * q.z + y * q.w + z * q.x,
            w * q.z + x * q.y - y * q.x + z * q.w,
            w * q.w - x * q.x - y * q.y - z * q.z};
  }

  static quat from_axis(vec3 a, float ang) {
    float s = std::sin(ang * 0.5f);
    return {a.x * s, a.y * s, a.z * s, std::cos(ang * 0.5f)};
  }

  static quat from_to(vec3 a, vec3 b) {
    float d = vec3::dot(a, b);
    vec3 axis = vec3::cross(a, b);
    if (d < -0.9999f)
      return {0, 1, 0, 0};
    float s = std::sqrt((1.0f + d) * 2.0f), inv_s = 1.0f / s;
    return {axis.x * inv_s, axis.y * inv_s, axis.z * inv_s, s * 0.5f};
  }

  static quat look_at(vec3 eye, vec3 target, vec3 up) {
    vec3 f = (target - eye).normalize();
    vec3 r = vec3::cross(f, up).normalize();
    vec3 u = vec3::cross(r, f);
    float m00 = r.x, m11 = u.y, m22 = -f.z, tr = m00 + m11 + m22;
    if (tr > 0) {
      float s = std::sqrt(tr + 1.0f) * 2.0f;
      return {(u.z - (-f.y)) / s, ((-f.x) - r.z) / s, (r.y - u.x) / s,
              0.25f * s};
    } else if ((m00 > m11) && (m00 > m22)) {
      float s = std::sqrt(1.0f + m00 - m11 - m22) * 2.0f;
      return {0.25f * s, (r.y + u.x) / s, ((-f.x) + r.z) / s,
              (u.z - (-f.y)) / s};
    } else if (m11 > m22) {
      float s = std::sqrt(1.0f + m11 - m00 - m22) * 2.0f;
      return {(r.y + u.x) / s, 0.25f * s, (u.z + (-f.y)) / s,
              ((-f.x) - r.z) / s};
    } else {
      float s = std::sqrt(1.0f + m22 - m00 - m11) * 2.0f;
      return {((-f.x) + r.z) / s, (u.z + (-f.y)) / s, 0.25f * s,
              (r.y - u.x) / s};
    }
  }

  vec3 rotate(vec3 v_in) const {
    vec3 qv(x, y, z), t = vec3::cross(qv, v_in) * 2.0f;
    return v_in + t * w + vec3::cross(qv, t);
  }

  mat4 to_mat() const {
    mat4 r;
    float x2 = x + x, y2 = y + y, z2 = z + z, xx = x * x2, xy = x * y2,
          xz = x * z2, yy = y * y2, yz = y * z2, zz = z * z2, wx = w * x2,
          wy = w * y2, wz = w * z2;
    r.m[0] = 1 - (yy + zz);
    r.m[4] = xy - wz;
    r.m[8] = xz + wy;
    r.m[1] = xy + wz;
    r.m[5] = 1 - (xx + zz);
    r.m[9] = yz - wx;
    r.m[2] = xz - wy;
    r.m[6] = yz + wx;
    r.m[10] = 1 - (xx + yy);
    return r;
  }
};

inline quat slerp(quat a, quat b, float t) {
  float d = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
  if (d < 0) {
    b = b * -1.0f;
    d = -d;
  }
  if (d > 0.9995f) { // Linear fall-back
    quat r;
    for (int i = 0; i < 4; ++i)
      r.v[i] = a.v[i] + (b.v[i] - a.v[i]) * t;
    return r;
  }
  float th0 = std::acos(d), th = th0 * t, s0 = std::sin(th0),
        s1 = std::sin(th) / s0, s2 = std::sin(th0 - th) / s0;
  return a * s2 + b * s1;
}
#endif /* defined(USE_QUAT) */

template <typename T> inline T lerp(const T& a, const T& b, float t) {
  return a + (b - a) * t;
}

#if defined(USE_EASING)
namespace ease {
inline float out_cubic(float t) {
  return 1.0f - std::pow(1.0f - t, 3.0f);
}
inline float in_out_quad(float t) {
  return t < 0.5f ? 2.0f * t * t
                  : 1.0f - std::pow(-2.0f * t + 2.0f, 2.0f) / 2.0f;
}
inline float out_expo(float t) {
  return t == 1.0f ? 1.0f : 1.0f - std::pow(2.0f, -10.0f * t);
}
} // namespace ease
#endif /* defined(USE_EASING) */

#if defined(USE_SPRING)
namespace spring {
template <typename T>
void solve(T& current, T& velocity, const T& target, float smooth_time,
           float dt) {
  float omega = 2.0f / smooth_time;
  float x = omega * dt;
  float exp = 1.0f / (1.0f + x + 0.48f * x * x + 0.235f * x * x * x);
  T change = current - target;
  T temp = (velocity + change * omega) * dt;
  velocity = (velocity - temp * omega) * exp;
  current = target + (change + temp) * exp;
}
} // namespace spring
#endif /* defined(USE_SPRING) */