// linalg.hh — 線型代数チュートリアルシリーズで作る数学ライブラリ
// Math library built throughout the Linear Algebra tutorial series.
// https://technicalsuwako.moe/tutorial/linalg  /  https://technicalsuwako.moe/tutorial/linalg-en
//
// 規約 / Conventions:
//   * 右手座標系 / right-handed coordinate system
//   * 列ベクトル、行列は左から掛ける / column vectors, matrices multiply from the left: v' = M * v
//   * 列優先メモリ配置（GLSLと同じ） / column-major storage (same as GLSL)
//   * 角度はラジアン / angles are in radians
#pragma once

#include <cmath>

//------------------------------------------
// 角度 / Angles (chapter 1)
//------------------------------------------

constexpr float PI = 3.14159265358979323846f;

// 度からラジアンへ / degrees to radians
constexpr float radians(float deg) { return deg * PI / 180.0f; }

// ラジアンから度へ / radians to degrees
constexpr float degrees(float rad) { return rad * 180.0f / PI; }

//------------------------------------------
// ベクトル / Vectors (chapters 2–5)
//------------------------------------------

struct Vec2 {
  float x = 0.0f, y = 0.0f;
};

struct Vec3 {
  float x = 0.0f, y = 0.0f, z = 0.0f;
};

struct Vec4 {
  float x = 0.0f, y = 0.0f, z = 0.0f, w = 0.0f;
};

// 足し算・引き算 / addition and subtraction (chapter 2)
inline Vec2 operator+(Vec2 a, Vec2 b) { return {a.x + b.x, a.y + b.y}; }
inline Vec2 operator-(Vec2 a, Vec2 b) { return {a.x - b.x, a.y - b.y}; }
inline Vec3 operator+(Vec3 a, Vec3 b) { return {a.x + b.x, a.y + b.y, a.z + b.z}; }
inline Vec3 operator-(Vec3 a, Vec3 b) { return {a.x - b.x, a.y - b.y, a.z - b.z}; }
inline Vec4 operator+(Vec4 a, Vec4 b) { return {a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w}; }
inline Vec4 operator-(Vec4 a, Vec4 b) { return {a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w}; }
inline Vec3 operator-(Vec3 v) { return {-v.x, -v.y, -v.z}; }

// スカラー倍 / scalar multiplication (chapter 2)
inline Vec2 operator*(float s, Vec2 v) { return {s * v.x, s * v.y}; }
inline Vec3 operator*(float s, Vec3 v) { return {s * v.x, s * v.y, s * v.z}; }
inline Vec4 operator*(float s, Vec4 v) { return {s * v.x, s * v.y, s * v.z, s * v.w}; }
inline Vec3 operator*(Vec3 v, float s) { return s * v; }
inline Vec3 operator/(Vec3 v, float s) { return {v.x / s, v.y / s, v.z / s}; }

// 線型補間 / linear interpolation (chapter 2)
inline Vec3 lerp(Vec3 a, Vec3 b, float t) { return a + t * (b - a); }

// 内積 / dot product (chapter 4)
inline float dot(Vec2 a, Vec2 b) { return a.x * b.x + a.y * b.y; }
inline float dot(Vec3 a, Vec3 b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
inline float dot(Vec4 a, Vec4 b) { return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; }

// 長さ・距離・正規化 / length, distance, normalization (chapter 3)
inline float length(Vec2 v) { return std::sqrt(v.x * v.x + v.y * v.y); }
inline float length(Vec3 v) { return std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); }
inline float distance(Vec3 a, Vec3 b) { return length(b - a); }

inline Vec3 normalize(Vec3 v) {
  float len = length(v);
  if (len == 0.0f) return {0.0f, 0.0f, 0.0f}; // ゼロベクトルは正規化出来ない / cannot normalize zero
  return (1.0f / len) * v;
}

// 外積 / cross product (chapter 5)
inline 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,
  };
}

//------------------------------------------
// 行列 / Matrices (chapters 6–10)
//------------------------------------------

// 4×4行列。列優先：c[列]が一つの列ベクトル。
// 4×4 matrix. Column-major: c[column] is one column vector.
struct Mat4 {
  Vec4 c[4];

  // 行row・列colの要素（0始まり） / element at row, col (0-based)
  float &at(int row, int col) {
    float *p = &c[col].x;
    return p[row];
  }
  float at(int row, int col) const {
    const float *p = &c[col].x;
    return p[row];
  }

  static Mat4 identity() {
    Mat4 m;
    m.c[0] = {1, 0, 0, 0};
    m.c[1] = {0, 1, 0, 0};
    m.c[2] = {0, 0, 1, 0};
    m.c[3] = {0, 0, 0, 1};
    return m;
  }
};

// 行列×ベクトル：列の線型結合 / matrix × vector: a linear combination of the columns (chapter 6)
inline Vec4 operator*(const Mat4 &m, Vec4 v) {
  return v.x * m.c[0] + v.y * m.c[1] + v.z * m.c[2] + v.w * m.c[3];
}

// 行列×行列 / matrix × matrix (chapter 7)
inline Mat4 operator*(const Mat4 &a, const Mat4 &b) {
  Mat4 r;
  for (int i = 0; i < 4; ++i) r.c[i] = a * b.c[i];
  return r;
}

// 転置 / transpose (chapter 7)
inline Mat4 transpose(const Mat4 &m) {
  Mat4 r;
  for (int row = 0; row < 4; ++row)
    for (int col = 0; col < 4; ++col)
      r.at(row, col) = m.at(col, row);
  return r;
}

// 拡大縮小 / scale (chapter 8)
inline Mat4 scale(Vec3 s) {
  Mat4 m = Mat4::identity();
  m.at(0, 0) = s.x;
  m.at(1, 1) = s.y;
  m.at(2, 2) = s.z;
  return m;
}

// 軸周りの回転 / rotation around the axes (chapter 8)
inline Mat4 rotateX(float angle) {
  float c = std::cos(angle), s = std::sin(angle);
  Mat4 m = Mat4::identity();
  m.at(1, 1) = c; m.at(1, 2) = -s;
  m.at(2, 1) = s; m.at(2, 2) = c;
  return m;
}

inline Mat4 rotateY(float angle) {
  float c = std::cos(angle), s = std::sin(angle);
  Mat4 m = Mat4::identity();
  m.at(0, 0) = c;  m.at(0, 2) = s;
  m.at(2, 0) = -s; m.at(2, 2) = c;
  return m;
}

inline Mat4 rotateZ(float angle) {
  float c = std::cos(angle), s = std::sin(angle);
  Mat4 m = Mat4::identity();
  m.at(0, 0) = c; m.at(0, 1) = -s;
  m.at(1, 0) = s; m.at(1, 1) = c;
  return m;
}

// 任意軸周りの回転（ロドリゲスの回転公式） / rotation around any unit axis (Rodrigues) (chapter 13)
inline Mat4 rotate(Vec3 axis, float angle) {
  Vec3 a = normalize(axis);
  float c = std::cos(angle), s = std::sin(angle), t = 1.0f - c;
  Mat4 m = Mat4::identity();
  m.at(0, 0) = t * a.x * a.x + c;
  m.at(0, 1) = t * a.x * a.y - s * a.z;
  m.at(0, 2) = t * a.x * a.z + s * a.y;
  m.at(1, 0) = t * a.x * a.y + s * a.z;
  m.at(1, 1) = t * a.y * a.y + c;
  m.at(1, 2) = t * a.y * a.z - s * a.x;
  m.at(2, 0) = t * a.x * a.z - s * a.y;
  m.at(2, 1) = t * a.y * a.z + s * a.x;
  m.at(2, 2) = t * a.z * a.z + c;
  return m;
}

// 平行移動 / translation (chapter 9)
inline Mat4 translate(Vec3 t) {
  Mat4 m = Mat4::identity();
  m.c[3] = {t.x, t.y, t.z, 1.0f};
  return m;
}

// 行列式（余因子展開） / determinant (cofactor expansion) (chapter 10)
inline float determinant(const Mat4 &m) {
  auto a = [&](int r, int c) { return m.at(r, c); };
  float s0 = a(0,0) * a(1,1) - a(1,0) * a(0,1);
  float s1 = a(0,0) * a(1,2) - a(1,0) * a(0,2);
  float s2 = a(0,0) * a(1,3) - a(1,0) * a(0,3);
  float s3 = a(0,1) * a(1,2) - a(1,1) * a(0,2);
  float s4 = a(0,1) * a(1,3) - a(1,1) * a(0,3);
  float s5 = a(0,2) * a(1,3) - a(1,2) * a(0,3);
  float c5 = a(2,2) * a(3,3) - a(3,2) * a(2,3);
  float c4 = a(2,1) * a(3,3) - a(3,1) * a(2,3);
  float c3 = a(2,1) * a(3,2) - a(3,1) * a(2,2);
  float c2 = a(2,0) * a(3,3) - a(3,0) * a(2,3);
  float c1 = a(2,0) * a(3,2) - a(3,0) * a(2,2);
  float c0 = a(2,0) * a(3,1) - a(3,0) * a(2,1);
  return s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0;
}

// 逆行列 / inverse matrix (chapter 10)
// 行列式が0なら逆行列は存在しない。okにfalseを入れて単位行列を返す。
// If the determinant is 0 there is no inverse: sets ok to false and returns identity.
inline Mat4 inverse(const Mat4 &m, bool *ok = nullptr) {
  auto a = [&](int r, int c) { return m.at(r, c); };
  float s0 = a(0,0) * a(1,1) - a(1,0) * a(0,1);
  float s1 = a(0,0) * a(1,2) - a(1,0) * a(0,2);
  float s2 = a(0,0) * a(1,3) - a(1,0) * a(0,3);
  float s3 = a(0,1) * a(1,2) - a(1,1) * a(0,2);
  float s4 = a(0,1) * a(1,3) - a(1,1) * a(0,3);
  float s5 = a(0,2) * a(1,3) - a(1,2) * a(0,3);
  float c5 = a(2,2) * a(3,3) - a(3,2) * a(2,3);
  float c4 = a(2,1) * a(3,3) - a(3,1) * a(2,3);
  float c3 = a(2,1) * a(3,2) - a(3,1) * a(2,2);
  float c2 = a(2,0) * a(3,3) - a(3,0) * a(2,3);
  float c1 = a(2,0) * a(3,2) - a(3,0) * a(2,2);
  float c0 = a(2,0) * a(3,1) - a(3,0) * a(2,1);
  float det = s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0;

  if (std::fabs(det) < 1e-8f) {
    if (ok) *ok = false;
    return Mat4::identity();
  }
  if (ok) *ok = true;

  float inv = 1.0f / det;
  Mat4 r;
  r.at(0,0) = ( a(1,1) * c5 - a(1,2) * c4 + a(1,3) * c3) * inv;
  r.at(0,1) = (-a(0,1) * c5 + a(0,2) * c4 - a(0,3) * c3) * inv;
  r.at(0,2) = ( a(3,1) * s5 - a(3,2) * s4 + a(3,3) * s3) * inv;
  r.at(0,3) = (-a(2,1) * s5 + a(2,2) * s4 - a(2,3) * s3) * inv;
  r.at(1,0) = (-a(1,0) * c5 + a(1,2) * c2 - a(1,3) * c1) * inv;
  r.at(1,1) = ( a(0,0) * c5 - a(0,2) * c2 + a(0,3) * c1) * inv;
  r.at(1,2) = (-a(3,0) * s5 + a(3,2) * s2 - a(3,3) * s1) * inv;
  r.at(1,3) = ( a(2,0) * s5 - a(2,2) * s2 + a(2,3) * s1) * inv;
  r.at(2,0) = ( a(1,0) * c4 - a(1,1) * c2 + a(1,3) * c0) * inv;
  r.at(2,1) = (-a(0,0) * c4 + a(0,1) * c2 - a(0,3) * c0) * inv;
  r.at(2,2) = ( a(3,0) * s4 - a(3,1) * s2 + a(3,3) * s0) * inv;
  r.at(2,3) = (-a(2,0) * s4 + a(2,1) * s2 - a(2,3) * s0) * inv;
  r.at(3,0) = (-a(1,0) * c3 + a(1,1) * c1 - a(1,2) * c0) * inv;
  r.at(3,1) = ( a(0,0) * c3 - a(0,1) * c1 + a(0,2) * c0) * inv;
  r.at(3,2) = (-a(3,0) * s3 + a(3,1) * s1 - a(3,2) * s0) * inv;
  r.at(3,3) = ( a(2,0) * s3 - a(2,1) * s1 + a(2,2) * s0) * inv;
  return r;
}

//------------------------------------------
// カメラと投影 / Camera and projection (chapters 11–12)
//------------------------------------------

// ビュー行列：カメラがeyeからtargetを見る。カメラは自分の-Z方向を向く。
// View matrix: camera at eye looking at target. The camera looks down its own -Z axis.
inline Mat4 lookAt(Vec3 eye, Vec3 target, Vec3 up) {
  Vec3 f = normalize(target - eye); // 前 / forward
  Vec3 r = normalize(cross(f, up)); // 右 / right
  Vec3 u = cross(r, f);             // 上 / up

  Mat4 m = Mat4::identity();
  m.at(0, 0) = r.x;  m.at(0, 1) = r.y;  m.at(0, 2) = r.z;  m.at(0, 3) = -dot(r, eye);
  m.at(1, 0) = u.x;  m.at(1, 1) = u.y;  m.at(1, 2) = u.z;  m.at(1, 3) = -dot(u, eye);
  m.at(2, 0) = -f.x; m.at(2, 1) = -f.y; m.at(2, 2) = -f.z; m.at(2, 3) = dot(f, eye);
  return m;
}

// 正射影（OpenGL：NDCのzは-1〜1） / orthographic projection (OpenGL: NDC z in -1..1)
inline Mat4 orthographicGl(float left, float right, float bottom, float top, float n, float f) {
  Mat4 m = Mat4::identity();
  m.at(0, 0) = 2.0f / (right - left);
  m.at(1, 1) = 2.0f / (top - bottom);
  m.at(2, 2) = -2.0f / (f - n);
  m.at(0, 3) = -(right + left) / (right - left);
  m.at(1, 3) = -(top + bottom) / (top - bottom);
  m.at(2, 3) = -(f + n) / (f - n);
  return m;
}

// 透視投影（OpenGL：NDCのzは-1〜1、yは上向き） / perspective (OpenGL: NDC z in -1..1, y up)
// fovY：縦の視野角（ラジアン） / vertical field of view in radians
inline Mat4 perspectiveGl(float fovY, float aspect, float n, float f) {
  float t = 1.0f / std::tan(fovY * 0.5f);
  Mat4 m;
  m.at(0, 0) = t / aspect;
  m.at(1, 1) = t;
  m.at(2, 2) = -(f + n) / (f - n);
  m.at(2, 3) = -(2.0f * f * n) / (f - n);
  m.at(3, 2) = -1.0f;
  return m;
}

// 透視投影（Vulkan：NDCのzは0〜1、yは下向き） / perspective (Vulkan: NDC z in 0..1, y down)
inline Mat4 perspectiveVk(float fovY, float aspect, float n, float f) {
  float t = 1.0f / std::tan(fovY * 0.5f);
  Mat4 m;
  m.at(0, 0) = t / aspect;
  m.at(1, 1) = -t;
  m.at(2, 2) = f / (n - f);
  m.at(2, 3) = (f * n) / (n - f);
  m.at(3, 2) = -1.0f;
  return m;
}

//------------------------------------------
// クォータニオン / Quaternions (chapter 13)
//------------------------------------------

// q = w + xi + yj + zk
struct Quat {
  float w = 1.0f, x = 0.0f, y = 0.0f, z = 0.0f;

  // 単位軸axis周りにangleラジアン回転 / rotation of angle radians around a unit axis
  static Quat fromAxisAngle(Vec3 axis, float angle) {
    Vec3 a = normalize(axis);
    float s = std::sin(angle * 0.5f);
    return {std::cos(angle * 0.5f), a.x * s, a.y * s, a.z * s};
  }
};

// ハミルトン積 / Hamilton product
inline Quat operator*(Quat a, Quat b) {
  return {
    a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z,
    a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
    a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
    a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
  };
}

inline Quat conjugate(Quat q) { return {q.w, -q.x, -q.y, -q.z}; }

inline Quat normalize(Quat q) {
  float len = std::sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
  return {q.w / len, q.x / len, q.y / len, q.z / len};
}

// ベクトルを回転：v' = q v q* / rotate a vector: v' = q v q*
inline Vec3 rotate(Quat q, Vec3 v) {
  Quat p{0.0f, v.x, v.y, v.z};
  Quat r = q * p * conjugate(q);
  return {r.x, r.y, r.z};
}

// 単位クォータニオンから回転行列へ / unit quaternion to rotation matrix
inline Mat4 toMat4(Quat q) {
  float w = q.w, x = q.x, y = q.y, z = q.z;
  Mat4 m = Mat4::identity();
  m.at(0, 0) = 1 - 2 * (y * y + z * z);
  m.at(0, 1) = 2 * (x * y - w * z);
  m.at(0, 2) = 2 * (x * z + w * y);
  m.at(1, 0) = 2 * (x * y + w * z);
  m.at(1, 1) = 1 - 2 * (x * x + z * z);
  m.at(1, 2) = 2 * (y * z - w * x);
  m.at(2, 0) = 2 * (x * z - w * y);
  m.at(2, 1) = 2 * (y * z + w * x);
  m.at(2, 2) = 1 - 2 * (x * x + y * y);
  return m;
}

// 球面線型補間 / spherical linear interpolation
inline Quat slerp(Quat a, Quat b, float t) {
  float d = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
  if (d < 0.0f) { // 短い方の道を選ぶ / take the shorter path
    b = {-b.w, -b.x, -b.y, -b.z};
    d = -d;
  }
  if (d > 0.9995f) { // ほぼ同じ向き：線型補間で十分 / nearly equal: plain lerp is enough
    Quat r{a.w + t * (b.w - a.w), a.x + t * (b.x - a.x), a.y + t * (b.y - a.y), a.z + t * (b.z - a.z)};
    return normalize(r);
  }
  float theta = std::acos(d);
  float sa = std::sin((1.0f - t) * theta) / std::sin(theta);
  float sb = std::sin(t * theta) / std::sin(theta);
  return {sa * a.w + sb * b.w, sa * a.x + sb * b.x, sa * a.y + sb * b.y, sa * a.z + sb * b.z};
}