// 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(); }

#ifdef 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

#ifdef 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

#ifdef 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

#ifdef 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)

  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;
  }
};
#endif

#ifdef 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

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

#ifdef 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);
}
}
#endif

#ifdef 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;
}
}
#endif