// hmap/hmath.h - tiny dependency-free math types used by libhmap.
// Conventions: right-handed, +Y up, forward = -Z, column vectors (p' = M * p + t).
#pragma once
#include <algorithm>
#include <cmath>
#include <cstdint>

namespace hmap {

constexpr float kPi = 3.14159265358979323846f;
constexpr float kDeg2Rad = kPi / 180.0f;
constexpr float kRad2Deg = 180.0f / kPi;
constexpr float kPlaneEps = 0.01f;  // world-unit tolerance used by plane classification

struct Vec2 {
    float x = 0, y = 0;
    constexpr Vec2() = default;
    constexpr Vec2(float x_, float y_) : x(x_), y(y_) {}
    Vec2 operator+(const Vec2& o) const { return {x + o.x, y + o.y}; }
    Vec2 operator-(const Vec2& o) const { return {x - o.x, y - o.y}; }
    Vec2 operator*(float s) const { return {x * s, y * s}; }
};

struct Vec3 {
    float x = 0, y = 0, z = 0;
    constexpr Vec3() = default;
    constexpr Vec3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {}
    Vec3 operator+(const Vec3& o) const { return {x + o.x, y + o.y, z + o.z}; }
    Vec3 operator-(const Vec3& o) const { return {x - o.x, y - o.y, z - o.z}; }
    Vec3 operator*(float s) const { return {x * s, y * s, z * s}; }
    Vec3 operator/(float s) const { return {x / s, y / s, z / s}; }
    Vec3 operator-() const { return {-x, -y, -z}; }
    Vec3& operator+=(const Vec3& o) { x += o.x; y += o.y; z += o.z; return *this; }
    Vec3& operator-=(const Vec3& o) { x -= o.x; y -= o.y; z -= o.z; return *this; }
    Vec3& operator*=(float s) { x *= s; y *= s; z *= s; return *this; }
    float operator[](int i) const { return i == 0 ? x : (i == 1 ? y : z); }
    float& operator[](int i) { return i == 0 ? x : (i == 1 ? y : z); }
    bool operator==(const Vec3& o) const { return x == o.x && y == o.y && z == o.z; }
    bool operator!=(const Vec3& o) const { return !(*this == o); }
};

inline Vec3 operator*(float s, const Vec3& v) { return v * s; }
inline float dot(const Vec3& a, const Vec3& b) { return a.x * b.x + a.y * b.y + a.z * b.z; }
inline Vec3 cross(const Vec3& a, const 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};
}
inline float lengthSq(const Vec3& v) { return dot(v, v); }
inline float length(const Vec3& v) { return std::sqrt(dot(v, v)); }
inline Vec3 normalize(const Vec3& v) {
    float l = length(v);
    return l > 1e-12f ? v / l : Vec3(0, 0, 0);
}
inline Vec3 vmin(const Vec3& a, const Vec3& b) { return {std::min(a.x, b.x), std::min(a.y, b.y), std::min(a.z, b.z)}; }
inline Vec3 vmax(const Vec3& a, const Vec3& b) { return {std::max(a.x, b.x), std::max(a.y, b.y), std::max(a.z, b.z)}; }
inline Vec3 lerp(const Vec3& a, const Vec3& b, float t) { return a + (b - a) * t; }
inline float clampf(float v, float lo, float hi) { return v < lo ? lo : (v > hi ? hi : v); }

// Plane: dot(n, p) == d. Solids are on the BACK side (dot(n,p) < d); n points out of the solid.
struct Plane {
    Vec3 n{0, 1, 0};
    float d = 0;
    float dist(const Vec3& p) const { return dot(n, p) - d; }
    static Plane fromPoints(const Vec3& a, const Vec3& b, const Vec3& c) {
        Vec3 n = normalize(cross(b - a, c - a));
        return Plane{n, dot(n, a)};
    }
    static Plane fromNormalPoint(const Vec3& n, const Vec3& p) {
        Vec3 nn = normalize(n);
        return Plane{nn, dot(nn, p)};
    }
};

struct AABB {
    Vec3 mn{1e30f, 1e30f, 1e30f}, mx{-1e30f, -1e30f, -1e30f};
    bool empty() const { return mn.x > mx.x; }
    void add(const Vec3& p) { mn = vmin(mn, p); mx = vmax(mx, p); }
    void add(const AABB& b) { if (!b.empty()) { add(b.mn); add(b.mx); } }
    Vec3 center() const { return (mn + mx) * 0.5f; }
    Vec3 size() const { return mx - mn; }
    bool overlaps(const AABB& o, float eps = 0) const {
        return !(o.mn.x > mx.x + eps || o.mx.x < mn.x - eps || o.mn.y > mx.y + eps ||
                 o.mx.y < mn.y - eps || o.mn.z > mx.z + eps || o.mx.z < mn.z - eps);
    }
    bool contains(const Vec3& p) const {
        return p.x >= mn.x && p.x <= mx.x && p.y >= mn.y && p.y <= mx.y && p.z >= mn.z && p.z <= mx.z;
    }
    AABB expanded(float e) const { AABB r = *this; r.mn = mn - Vec3(e, e, e); r.mx = mx + Vec3(e, e, e); return r; }
};

// Slab test. Returns true when the ray hits; tNear may be negative when the origin is inside.
inline bool rayAABB(const Vec3& o, const Vec3& d, const AABB& b, float& tNear, float& tFar) {
    float t0 = -1e30f, t1 = 1e30f;
    for (int i = 0; i < 3; ++i) {
        if (std::fabs(d[i]) < 1e-9f) {
            if (o[i] < b.mn[i] || o[i] > b.mx[i]) return false;
        } else {
            float inv = 1.0f / d[i];
            float a = (b.mn[i] - o[i]) * inv, c = (b.mx[i] - o[i]) * inv;
            if (a > c) std::swap(a, c);
            t0 = std::max(t0, a);
            t1 = std::min(t1, c);
            if (t0 > t1) return false;
        }
    }
    tNear = t0;
    tFar = t1;
    return t1 >= 0;
}

// 3x4 affine matrix.
struct Mat34 {
    float m[3][4] = {{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}};

    static Mat34 identity() { return Mat34(); }
    static Mat34 translate(const Vec3& t) {
        Mat34 r;
        r.m[0][3] = t.x; r.m[1][3] = t.y; r.m[2][3] = t.z;
        return r;
    }
    static Mat34 scale(const Vec3& s) {
        Mat34 r;
        r.m[0][0] = s.x; r.m[1][1] = s.y; r.m[2][2] = s.z;
        return r;
    }
    static Mat34 rotate(const Vec3& axis, float deg) {  // Rodrigues, right-handed
        Vec3 a = normalize(axis);
        float c = std::cos(deg * kDeg2Rad), s = std::sin(deg * kDeg2Rad), t = 1 - c;
        Mat34 r;
        r.m[0][0] = t * a.x * a.x + c;       r.m[0][1] = t * a.x * a.y - s * a.z; r.m[0][2] = t * a.x * a.z + s * a.y;
        r.m[1][0] = t * a.x * a.y + s * a.z; r.m[1][1] = t * a.y * a.y + c;       r.m[1][2] = t * a.y * a.z - s * a.x;
        r.m[2][0] = t * a.x * a.z - s * a.y; r.m[2][1] = t * a.y * a.z + s * a.x; r.m[2][2] = t * a.z * a.z + c;
        return r;
    }
    // angles = (pitch, yaw, roll) degrees. R = Ry(yaw) * Rx(pitch) * Rz(roll). Forward (-Z) with
    // yaw 0 / pitch 0 stays -Z; +pitch looks up; +yaw turns left (counter-clockwise from above).
    static Mat34 fromEuler(const Vec3& a) {
        return rotate({0, 1, 0}, a.y) * rotate({1, 0, 0}, a.x) * rotate({0, 0, 1}, a.z);
    }
    Vec3 toEuler() const {  // inverse of fromEuler
        float sp = -m[1][2];
        float pitch = std::asin(clampf(sp, -1.0f, 1.0f));
        float yaw, roll;
        if (std::fabs(sp) < 0.99999f) {
            yaw = std::atan2(m[0][2], m[2][2]);
            roll = std::atan2(m[1][0], m[1][1]);
        } else {
            yaw = std::atan2(-m[2][0], m[0][0]);
            roll = 0;
        }
        return {pitch * kRad2Deg, yaw * kRad2Deg, roll * kRad2Deg};
    }
    // Euler angles for the FBX default XYZ order (R = Rz * Ry * Rx), degrees.
    Vec3 toEulerXYZ() const {
        float sb = -m[2][0];
        float b = std::asin(clampf(sb, -1.0f, 1.0f));
        float a, c;
        if (std::fabs(sb) < 0.99999f) {
            a = std::atan2(m[2][1], m[2][2]);
            c = std::atan2(m[1][0], m[0][0]);
        } else {
            a = std::atan2(-m[1][2], m[1][1]);
            c = 0;
        }
        return {a * kRad2Deg, b * kRad2Deg, c * kRad2Deg};
    }
    Vec3 point(const Vec3& p) const {
        return {m[0][0] * p.x + m[0][1] * p.y + m[0][2] * p.z + m[0][3],
                m[1][0] * p.x + m[1][1] * p.y + m[1][2] * p.z + m[1][3],
                m[2][0] * p.x + m[2][1] * p.y + m[2][2] * p.z + m[2][3]};
    }
    Vec3 vector(const Vec3& v) const {
        return {m[0][0] * v.x + m[0][1] * v.y + m[0][2] * v.z,
                m[1][0] * v.x + m[1][1] * v.y + m[1][2] * v.z,
                m[2][0] * v.x + m[2][1] * v.y + m[2][2] * v.z};
    }
    Vec3 translation() const { return {m[0][3], m[1][3], m[2][3]}; }
    // this * b : applies b first, then this.
    Mat34 operator*(const Mat34& b) const {
        Mat34 r;
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j)
                r.m[i][j] = m[i][0] * b.m[0][j] + m[i][1] * b.m[1][j] + m[i][2] * b.m[2][j];
            r.m[i][3] = m[i][0] * b.m[0][3] + m[i][1] * b.m[1][3] + m[i][2] * b.m[2][3] + m[i][3];
        }
        return r;
    }
    float det3() const {
        return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) -
               m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) +
               m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
    }
    bool inverse(Mat34& out) const {
        float det = det3();
        if (std::fabs(det) < 1e-12f) return false;
        float id = 1.0f / det;
        Mat34 r;
        r.m[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) * id;
        r.m[0][1] = (m[0][2] * m[2][1] - m[0][1] * m[2][2]) * id;
        r.m[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * id;
        r.m[1][0] = (m[1][2] * m[2][0] - m[1][0] * m[2][2]) * id;
        r.m[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * id;
        r.m[1][2] = (m[0][2] * m[1][0] - m[0][0] * m[1][2]) * id;
        r.m[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) * id;
        r.m[2][1] = (m[0][1] * m[2][0] - m[0][0] * m[2][1]) * id;
        r.m[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * id;
        Vec3 t = translation();
        Vec3 it = r.vector(t);
        r.m[0][3] = -it.x; r.m[1][3] = -it.y; r.m[2][3] = -it.z;
        out = r;
        return true;
    }
    // Transforms a plane by this affine map (normal uses the inverse transpose).
    Plane transformPlane(const Plane& pl) const {
        Mat34 inv;
        if (!inverse(inv)) return pl;
        Vec3 n(inv.m[0][0] * pl.n.x + inv.m[1][0] * pl.n.y + inv.m[2][0] * pl.n.z,
               inv.m[0][1] * pl.n.x + inv.m[1][1] * pl.n.y + inv.m[2][1] * pl.n.z,
               inv.m[0][2] * pl.n.x + inv.m[1][2] * pl.n.y + inv.m[2][2] * pl.n.z);
        float d = pl.d + dot(n, translation());
        float l = length(n);
        if (l < 1e-12f) return pl;
        return Plane{n / l, d / l};
    }
};

}  // namespace hmap