#include "Physics.h"
#include <algorithm>
#include <cmath>
#include <glm/gtc/constants.hpp>

namespace phys {

namespace {

constexpr float kSleepLinearSq  = 0.05f * 0.05f;
constexpr float kSleepAngularSq = 0.1f  * 0.1f;
constexpr float kSleepTime      = 0.5f;
constexpr float kPenetrationSlop = 0.005f;
constexpr float kBaumgarte       = 0.2f;

// --- Small helpers -------------------------------------------------------

glm::mat3 BoxInertiaTensor(float mass, const glm::vec3& h) {
    // h = half-extents; full size = 2h.
    float x2 = (2.0f * h.x) * (2.0f * h.x);
    float y2 = (2.0f * h.y) * (2.0f * h.y);
    float z2 = (2.0f * h.z) * (2.0f * h.z);
    float k = mass / 12.0f;
    return glm::mat3(
        k * (y2 + z2), 0.0f, 0.0f,
        0.0f, k * (x2 + z2), 0.0f,
        0.0f, 0.0f, k * (x2 + y2)
    );
}

glm::mat3 SphereInertiaTensor(float mass, float radius) {
    float v = (2.0f / 5.0f) * mass * radius * radius;
    return glm::mat3(v, 0, 0,  0, v, 0,  0, 0, v);
}

glm::mat3 Inverse3(const glm::mat3& m) {
    // glm::inverse handles this, but we keep local to make zero-diag safe.
    glm::mat3 r(0.0f);
    if (m[0][0] > 0) r[0][0] = 1.0f / m[0][0];
    if (m[1][1] > 0) r[1][1] = 1.0f / m[1][1];
    if (m[2][2] > 0) r[2][2] = 1.0f / m[2][2];
    return r;
}

void RecomputeInvInertiaWorld(Body& b) {
    if (b.is_static || b.inv_mass == 0.0f) {
        b.inv_inertia_world = glm::mat3(0.0f);
        return;
    }
    glm::mat3 R = glm::mat3_cast(b.orientation);
    b.inv_inertia_world = R * b.inv_inertia_local * glm::transpose(R);
}

void ComputeBasis(const glm::vec3& n, glm::vec3& t1, glm::vec3& t2) {
    if (std::abs(n.x) >= 0.57735f)
        t1 = glm::normalize(glm::vec3(n.y, -n.x, 0.0f));
    else
        t1 = glm::normalize(glm::vec3(0.0f, n.z, -n.y));
    t2 = glm::cross(n, t1);
}

glm::vec3 VelAtPoint(const Body& b, const glm::vec3& r) {
    return b.linear_velocity + glm::cross(b.angular_velocity, r);
}

// --- Collision tests -----------------------------------------------------

// Returns support point of an OBB in world space along direction d.
glm::vec3 BoxSupport(const Body& box, const glm::vec3& d) {
    glm::mat3 R = glm::mat3_cast(box.orientation);
    glm::vec3 ld = glm::transpose(R) * d;
    glm::vec3 lp(
        ld.x >= 0 ? box.half_extents.x : -box.half_extents.x,
        ld.y >= 0 ? box.half_extents.y : -box.half_extents.y,
        ld.z >= 0 ? box.half_extents.z : -box.half_extents.z
    );
    return box.position + R * lp;
}

}  // namespace

int World::AddBody(const Body& b) {
    int handle = (int)bodies_.size();
    bodies_.push_back(b);
    Body& nb = bodies_.back();
    nb.handle = handle;
    nb.alive = true;

    if (nb.is_static) nb.inv_mass = 0.0f;

    if (nb.shape == Shape::Sphere && nb.inv_mass > 0.0f) {
        float mass = 1.0f / nb.inv_mass;
        nb.inv_inertia_local = Inverse3(SphereInertiaTensor(mass, nb.half_extents.x));
    } else if (nb.shape == Shape::Box && nb.inv_mass > 0.0f) {
        float mass = 1.0f / nb.inv_mass;
        nb.inv_inertia_local = Inverse3(BoxInertiaTensor(mass, nb.half_extents));
    } else {
        nb.inv_inertia_local = glm::mat3(0.0f);
    }
    RecomputeInvInertiaWorld(nb);
    return handle;
}

void World::RemoveBody(int handle) {
    if (handle < 0 || handle >= (int)bodies_.size()) return;
    bodies_[handle].alive = false;
}

Body* World::Get(int handle) {
    if (handle < 0 || handle >= (int)bodies_.size()) return nullptr;
    return bodies_[handle].alive ? &bodies_[handle] : nullptr;
}
const Body* World::Get(int handle) const {
    if (handle < 0 || handle >= (int)bodies_.size()) return nullptr;
    return bodies_[handle].alive ? &bodies_[handle] : nullptr;
}

void World::Step(float real_dt) {
    if (!enabled) return;
    if (real_dt > 0.25f) real_dt = 0.25f;  // avoid spiral of death
    accumulator_ += real_dt;
    while (accumulator_ >= fixed_dt) {
        IntegrateForces(fixed_dt);
        DetectCollisions();
        PrepareSolver(fixed_dt);
        WarmStart();
        for (int i = 0; i < solver_iterations; ++i) SolveVelocities();
        IntegrateVelocities(fixed_dt);
        UpdateSleep(fixed_dt);
        prev_contacts_ = contacts_;
        accumulator_ -= fixed_dt;
    }
}

void World::IntegrateForces(float dt) {
    static const int kBoxSigns[8][3] = {
        {-1,-1,-1},{ 1,-1,-1},{-1, 1,-1},{ 1, 1,-1},
        {-1,-1, 1},{ 1,-1, 1},{-1, 1, 1},{ 1, 1, 1}
    };

    for (auto& b : bodies_) {
        if (!b.alive || b.is_static || b.inv_mass == 0.0f) continue;
        if (b.sleeping) continue;

        glm::vec3 net_accel = gravity + b.force_accum * b.inv_mass;
        glm::vec3 total_torque = b.torque_accum;
        float submerged_frac_agg = 0.0f;

        if (water_enabled && b.shape != Shape::Plane) {
            if (b.shape == Shape::Sphere) {
                float r = b.half_extents.x;
                float wy = SampleWaterY(b.position.x, b.position.z);
                float d = wy - (b.position.y - r);
                if (d > 0.0f) {
                    float h = std::min(d, 2.0f * r);
                    float cap_vol = glm::pi<float>() * h * h * (3.0f * r - h) / 3.0f;
                    float full_vol = (4.0f / 3.0f) * glm::pi<float>() * r * r * r;
                    float frac = cap_vol / full_vol;
                    net_accel += -gravity * water_density * frac;
                    submerged_frac_agg = frac;
                }
            } else {  // Box — sample 8 corners against the wave surface
                glm::mat3 R = glm::mat3_cast(b.orientation);
                float body_mass = 1.0f / b.inv_mass;
                glm::vec3 total_force(0.0f);
                float diag_y = 2.0f * b.half_extents.y;
                int submerged_count = 0;
                for (int i = 0; i < 8; ++i) {
                    glm::vec3 local = b.half_extents *
                        glm::vec3(kBoxSigns[i][0], kBoxSigns[i][1], kBoxSigns[i][2]);
                    glm::vec3 corner = b.position + R * local;
                    float wy = SampleWaterY(corner.x, corner.z);
                    float d = wy - corner.y;
                    if (d <= 0.0f) continue;
                    ++submerged_count;
                    float frac = std::min(1.0f, d / diag_y);
                    submerged_frac_agg += frac / 8.0f;
                    glm::vec3 accel_i = -gravity * water_density * (frac / 8.0f);
                    glm::vec3 force_i = accel_i * body_mass;
                    glm::vec3 r = corner - b.position;
                    total_force  += force_i;
                    total_torque += glm::cross(r, force_i);
                }
                net_accel += total_force * b.inv_mass;
                (void)submerged_count;
            }

            // Surface current from wind
            if (submerged_frac_agg > 0.0f && wind_speed > 0.0f) {
                glm::vec3 wind3(wind_dir_xz.x, 0.0f, wind_dir_xz.y);
                float lw = glm::length(wind3);
                if (lw > 1e-5f) wind3 /= lw;
                net_accel += wind3 * wind_speed * current_coef * submerged_frac_agg;
            }
            // Directional water flow (rivers, waterfalls)
            if (submerged_frac_agg > 0.0f && water_flow_speed > 0.0f) {
                glm::vec3 flow3(water_flow_dir.x, 0.0f, water_flow_dir.y);
                float lf = glm::length(flow3);
                if (lf > 1e-5f) flow3 /= lf;
                net_accel += flow3 * water_flow_speed * submerged_frac_agg;
            }

            if (submerged_frac_agg > 0.0f) {
                b.linear_velocity  *= std::max(0.0f,
                    1.0f - water_drag_linear  * submerged_frac_agg * dt);
                b.angular_velocity *= std::max(0.0f,
                    1.0f - water_drag_angular * submerged_frac_agg * dt);
            }
        }

        b.linear_velocity  += net_accel * dt;
        b.angular_velocity += b.inv_inertia_world * total_torque * dt;
        b.linear_velocity  *= std::max(0.0f, 1.0f - b.linear_damping  * dt);
        b.angular_velocity *= std::max(0.0f, 1.0f - b.angular_damping * dt);
        b.force_accum  = glm::vec3(0.0f);
        b.torque_accum = glm::vec3(0.0f);
    }
}

float World::SampleWaterY(float x, float z) const {
    // MUST stay in sync with the water vertex shader's Gerstner arrays.
    static const float DIRS[10][2] = {
        { 1.00f,  0.00f},
        { 0.71f, -0.71f},
        {-0.41f,  0.91f},
        {-0.85f, -0.52f},
        { 0.30f,  0.95f},
        { 0.88f,  0.47f},
        {-0.94f,  0.33f},
        { 0.60f, -0.80f},
        {-0.15f, -0.99f},
        { 0.43f,  0.90f}
    };
    static const float AMP_SCALE[10]   = {
        0.70f, 0.46f, 0.32f, 0.25f, 0.18f, 0.14f, 0.11f, 0.08f, 0.06f, 0.05f };
    static const float LEN_SCALE[10]   = {
        1.00f, 0.77f, 0.59f, 0.46f, 0.36f, 0.28f, 0.22f, 0.17f, 0.13f, 0.10f };
    static const float SPEED_SCALE[10] = {
        1.00f, 1.23f, 0.82f, 1.37f, 0.91f, 1.19f, 0.75f, 1.48f, 1.07f, 0.88f };
    float y = water_level;
    for (int i = 0; i < 10; ++i) {
        float dx = DIRS[i][0], dz = DIRS[i][1];
        float nrm = std::sqrt(dx * dx + dz * dz);
        dx /= nrm; dz /= nrm;
        float L = water_wavelength * LEN_SCALE[i];
        float w = 2.0f * glm::pi<float>() / L;
        float A = water_amplitude * AMP_SCALE[i] * water_amp_scale;
        float phi = water_time * water_speed * SPEED_SCALE[i] * std::sqrt(9.81f * w);
        float theta = (dx * x + dz * z) * w + phi;
        y += A * std::sin(theta);
    }
    return y;
}

void World::DetectCollisions() {
    contacts_.clear();
    const int n = (int)bodies_.size();
    for (int i = 0; i < n; ++i) {
        if (!bodies_[i].alive) continue;
        for (int j = i + 1; j < n; ++j) {
            if (!bodies_[j].alive) continue;
            const Body& A = bodies_[i];
            const Body& B = bodies_[j];
            if (A.is_static && B.is_static) continue;
            if (A.sleeping && B.sleeping)   continue;
            TestPair(i, j);
        }
    }
}

void World::TestPair(int ia, int ib) {
    const Body& A = bodies_[ia];
    const Body& B = bodies_[ib];
    // Normalize ordering so Sphere is always 'a' for asymmetric pairs.
    auto swap01 = [](Shape s0, Shape s1) {
        return std::make_pair(s0, s1);
    };
    auto [sa, sb] = swap01(A.shape, B.shape);

    if (sa == Shape::Sphere && sb == Shape::Sphere) {
        SphereSphere(A, B, ia, ib);
    } else if (sa == Shape::Sphere && sb == Shape::Plane) {
        SpherePlane(A, B, ia, ib);
    } else if (sa == Shape::Plane && sb == Shape::Sphere) {
        SpherePlane(B, A, ib, ia);
    } else if (sa == Shape::Sphere && sb == Shape::Box) {
        SphereBox(A, B, ia, ib);
    } else if (sa == Shape::Box && sb == Shape::Sphere) {
        SphereBox(B, A, ib, ia);
    } else if (sa == Shape::Box && sb == Shape::Plane) {
        BoxPlane(A, B, ia, ib);
    } else if (sa == Shape::Plane && sb == Shape::Box) {
        BoxPlane(B, A, ib, ia);
    } else if (sa == Shape::Box && sb == Shape::Box) {
        BoxBox(A, B, ia, ib);
    }
    // Plane-plane intentionally skipped.
}

void World::SphereSphere(const Body& A, const Body& B, int ia, int ib) {
    glm::vec3 d = B.position - A.position;
    float r = A.half_extents.x + B.half_extents.x;
    float d2 = glm::dot(d, d);
    if (d2 >= r * r) return;
    float dist = std::sqrt(std::max(d2, 1e-12f));
    glm::vec3 n = (dist > 1e-6f) ? d / dist : glm::vec3(0.0f, 1.0f, 0.0f);
    Contact c;
    c.a = ia; c.b = ib;
    c.normal = n;
    c.penetration = r - dist;
    c.point = A.position + n * A.half_extents.x;
    contacts_.push_back(c);
}

void World::SpherePlane(const Body& s, const Body& p, int is, int ip) {
    float d = glm::dot(p.plane_normal, s.position) - p.plane_offset;
    float pen = s.half_extents.x - d;
    if (pen <= 0.0f) return;
    Contact c;
    c.a = is; c.b = ip;
    c.normal = -p.plane_normal;  // A (sphere) → B (plane)
    c.penetration = pen;
    c.point = s.position - p.plane_normal * s.half_extents.x;
    contacts_.push_back(c);
}

void World::SphereBox(const Body& s, const Body& box, int is, int ib) {
    glm::mat3 R = glm::mat3_cast(box.orientation);
    glm::vec3 rel = glm::transpose(R) * (s.position - box.position);
    glm::vec3 closest = glm::clamp(rel, -box.half_extents, box.half_extents);
    glm::vec3 diff = rel - closest;
    float d2 = glm::dot(diff, diff);
    float r = s.half_extents.x;
    if (d2 >= r * r) return;

    glm::vec3 n_local;
    float dist;
    if (d2 > 1e-12f) {
        dist = std::sqrt(d2);
        n_local = diff / dist;
    } else {
        // Sphere center inside box — pick axis of minimum penetration.
        glm::vec3 pen_axes = box.half_extents - glm::abs(rel);
        if (pen_axes.x < pen_axes.y && pen_axes.x < pen_axes.z) {
            n_local = glm::vec3(rel.x < 0 ? -1.0f : 1.0f, 0, 0);
            dist = -pen_axes.x;
        } else if (pen_axes.y < pen_axes.z) {
            n_local = glm::vec3(0, rel.y < 0 ? -1.0f : 1.0f, 0);
            dist = -pen_axes.y;
        } else {
            n_local = glm::vec3(0, 0, rel.z < 0 ? -1.0f : 1.0f);
            dist = -pen_axes.z;
        }
    }

    glm::vec3 n_world = R * n_local;   // points from box toward sphere center
    glm::vec3 contact_world = box.position + R * closest;

    Contact c;
    // Our convention: normal points from A(sphere) toward B(box)
    // If is < ib our 'a' is the sphere. But caller may pass s as 'A' regardless
    // of handle order. We push what DetectCollisions expects using (is, ib).
    c.a = is;
    c.b = ib;
    c.normal = -n_world;  // sphere -> box
    c.penetration = r - dist;
    c.point = contact_world;
    contacts_.push_back(c);
}

void World::BoxPlane(const Body& box, const Body& p, int ib, int ip) {
    glm::mat3 R = glm::mat3_cast(box.orientation);
    // For each of 8 vertices, test against plane. Push contact per-penetrating.
    static const glm::vec3 kSigns[8] = {
        {-1,-1,-1},{ 1,-1,-1},{-1, 1,-1},{ 1, 1,-1},
        {-1,-1, 1},{ 1,-1, 1},{-1, 1, 1},{ 1, 1, 1}
    };
    int added = 0;
    for (int i = 0; i < 8; ++i) {
        glm::vec3 lp = box.half_extents * kSigns[i];
        glm::vec3 wp = box.position + R * lp;
        float d = glm::dot(p.plane_normal, wp) - p.plane_offset;
        if (d < 0.0f) {
            Contact c;
            c.a = ib; c.b = ip;
            c.normal = -p.plane_normal;  // box -> plane
            c.penetration = -d;
            c.point = wp;
            contacts_.push_back(c);
            if (++added >= 4) break;  // manifold cap
        }
    }
}

// --- SAT Box-Box ---------------------------------------------------------
namespace {

struct BoxAxes {
    glm::vec3 c;
    glm::vec3 u[3];   // world-space basis columns of R (unit length)
    glm::vec3 h;      // half-extents
};

BoxAxes MakeAxes(const Body& box) {
    BoxAxes out;
    out.c = box.position;
    glm::mat3 R = glm::mat3_cast(box.orientation);
    out.u[0] = R[0]; out.u[1] = R[1]; out.u[2] = R[2];
    out.h = box.half_extents;
    return out;
}

float ProjectOBB(const BoxAxes& a, const glm::vec3& axis) {
    return a.h.x * std::abs(glm::dot(a.u[0], axis))
         + a.h.y * std::abs(glm::dot(a.u[1], axis))
         + a.h.z * std::abs(glm::dot(a.u[2], axis));
}

// Clip polygon by half-space n·x <= d (returns clipped vertices).
std::vector<glm::vec3> ClipPoly(const std::vector<glm::vec3>& in,
                                const glm::vec3& n, float d) {
    std::vector<glm::vec3> out;
    out.reserve(in.size() + 1);
    if (in.empty()) return out;
    for (size_t i = 0; i < in.size(); ++i) {
        const glm::vec3& cur = in[i];
        const glm::vec3& prev = in[(i + in.size() - 1) % in.size()];
        float dc = glm::dot(n, cur) - d;
        float dp = glm::dot(n, prev) - d;
        bool cur_in = dc <= 0.0f;
        bool prev_in = dp <= 0.0f;
        if (prev_in ^ cur_in) {
            float t = dp / (dp - dc);
            out.push_back(prev + (cur - prev) * t);
        }
        if (cur_in) out.push_back(cur);
    }
    return out;
}

}  // namespace

void World::BoxBox(const Body& A, const Body& B, int ia, int ib) {
    BoxAxes a = MakeAxes(A);
    BoxAxes b = MakeAxes(B);

    glm::vec3 t = b.c - a.c;

    // Build matrix of dot products between A and B axes.
    float Rm[3][3], AbsR[3][3];
    const float kEps = 1e-6f;
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j) {
            Rm[i][j]   = glm::dot(a.u[i], b.u[j]);
            AbsR[i][j] = std::abs(Rm[i][j]) + kEps;
        }

    float t_a[3] = {
        glm::dot(t, a.u[0]), glm::dot(t, a.u[1]), glm::dot(t, a.u[2])
    };

    float best_overlap = std::numeric_limits<float>::max();
    int best_axis_type = -1;  // 0=A face, 1=B face, 2=edge cross
    int best_axis_index = 0;  // face index, or i*3+j for edge
    float best_sign = 1.0f;   // axis direction sign toward B
    glm::vec3 best_axis(0.0f);

    auto TestAxis = [&](const glm::vec3& L, float ra, float rb, float center_dist,
                        int type, int index) {
        float len = glm::length(L);
        if (len < 1e-8f) return true;  // parallel edges; skip
        float overlap = (ra + rb) - std::abs(center_dist);
        if (overlap < 0.0f) return false;
        // Normalize overlap per-axis so face axes compete fairly with edge axes.
        float o_norm = overlap / len;
        if (o_norm < best_overlap) {
            best_overlap = o_norm;
            best_axis_type = type;
            best_axis_index = index;
            best_axis = L / len;
            best_sign = (center_dist < 0) ? -1.0f : 1.0f;
        }
        return true;
    };

    // A's face axes
    for (int i = 0; i < 3; ++i) {
        float ra = a.h[i];
        float rb = b.h.x * AbsR[i][0] + b.h.y * AbsR[i][1] + b.h.z * AbsR[i][2];
        if (!TestAxis(a.u[i], ra, rb, t_a[i], 0, i)) return;
    }
    // B's face axes
    for (int j = 0; j < 3; ++j) {
        float ra = a.h.x * AbsR[0][j] + a.h.y * AbsR[1][j] + a.h.z * AbsR[2][j];
        float rb = b.h[j];
        float dist = glm::dot(t, b.u[j]);
        if (!TestAxis(b.u[j], ra, rb, dist, 1, j)) return;
    }
    // 9 edge-edge axes
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            glm::vec3 L = glm::cross(a.u[i], b.u[j]);
            int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
            int j1 = (j + 1) % 3, j2 = (j + 2) % 3;
            float ra = a.h[i1] * AbsR[i2][j] + a.h[i2] * AbsR[i1][j];
            float rb = b.h[j1] * AbsR[i][j2] + b.h[j2] * AbsR[i][j1];
            float dist = t_a[i2] * Rm[i1][j] - t_a[i1] * Rm[i2][j];
            if (!TestAxis(L, ra, rb, dist, 2, i * 3 + j)) return;
        }
    }

    if (best_axis_type < 0) return;

    // Ensure normal points from A to B.
    glm::vec3 n = best_axis * best_sign;
    if (glm::dot(n, t) < 0.0f) n = -n;

    // Face-face manifold via Sutherland–Hodgman clipping.
    if (best_axis_type == 0 || best_axis_type == 1) {
        const BoxAxes& ref  = (best_axis_type == 0) ? a : b;
        const BoxAxes& inc  = (best_axis_type == 0) ? b : a;
        glm::vec3 refN = (best_axis_type == 0) ? n : -n;  // points outward from ref

        // Reference face: the one whose outward normal = refN (one of ±ref.u[k])
        int ref_k = 0;
        float best_dot = -std::numeric_limits<float>::max();
        float ref_sign = 1.0f;
        for (int k = 0; k < 3; ++k) {
            float d0 = glm::dot(ref.u[k], refN);
            if (std::abs(d0) > best_dot) {
                best_dot = std::abs(d0);
                ref_k = k;
                ref_sign = (d0 >= 0) ? 1.0f : -1.0f;
            }
        }
        glm::vec3 refAxis = ref.u[ref_k] * ref_sign;
        glm::vec3 refCenter = ref.c + refAxis * ref.h[ref_k];

        // Incident face: pick incident box face whose normal is most opposite refAxis.
        int inc_k = 0;
        float min_dot = std::numeric_limits<float>::max();
        float inc_sign = 1.0f;
        for (int k = 0; k < 3; ++k) {
            float d0 = glm::dot(inc.u[k], refAxis);
            if (d0 < min_dot) {
                min_dot = d0;
                inc_k = k; inc_sign = 1.0f;
            }
            if (-d0 < min_dot) {
                min_dot = -d0;
                inc_k = k; inc_sign = -1.0f;
            }
        }

        // Build the incident face as 4 world-space vertices.
        int u = (inc_k + 1) % 3;
        int v = (inc_k + 2) % 3;
        glm::vec3 inc_center = inc.c + inc.u[inc_k] * (inc.h[inc_k] * inc_sign);
        glm::vec3 inc_u = inc.u[u] * inc.h[u];
        glm::vec3 inc_v = inc.u[v] * inc.h[v];
        std::vector<glm::vec3> poly = {
            inc_center - inc_u - inc_v,
            inc_center + inc_u - inc_v,
            inc_center + inc_u + inc_v,
            inc_center - inc_u + inc_v
        };

        // Clip against the 4 side planes of the reference face.
        int ru = (ref_k + 1) % 3;
        int rv = (ref_k + 2) % 3;
        glm::vec3 side_axes[4] = { ref.u[ru], -ref.u[ru], ref.u[rv], -ref.u[rv] };
        float side_d[4] = {
            glm::dot(ref.u[ru], refCenter) + ref.h[ru],
            -glm::dot(ref.u[ru], refCenter) + ref.h[ru],
            glm::dot(ref.u[rv], refCenter) + ref.h[rv],
            -glm::dot(ref.u[rv], refCenter) + ref.h[rv]
        };
        for (int s = 0; s < 4; ++s)
            poly = ClipPoly(poly, side_axes[s], side_d[s]);

        // Emit penetrating points (those behind the reference face plane).
        int emitted = 0;
        for (const auto& p : poly) {
            float d = glm::dot(refAxis, p - refCenter);
            if (d > 0.0f) continue;
            Contact c;
            c.a = ia; c.b = ib;
            c.normal = n;
            c.penetration = -d;
            c.point = p;
            contacts_.push_back(c);
            if (++emitted >= 4) break;
        }
        if (emitted == 0) {
            // Fallback: single-point contact at deepest witness along n.
            glm::vec3 pA = BoxSupport(A,  n);
            glm::vec3 pB = BoxSupport(B, -n);
            Contact c;
            c.a = ia; c.b = ib;
            c.normal = n;
            c.penetration = best_overlap;
            c.point = 0.5f * (pA + pB);
            contacts_.push_back(c);
        }
    } else {
        // Edge-edge — approximate with midpoint of nearest-point pair.
        glm::vec3 pA = BoxSupport(A,  n);
        glm::vec3 pB = BoxSupport(B, -n);
        Contact c;
        c.a = ia; c.b = ib;
        c.normal = n;
        c.penetration = best_overlap;
        c.point = 0.5f * (pA + pB);
        contacts_.push_back(c);
    }
}

// --- Solver --------------------------------------------------------------

void World::PrepareSolver(float dt) {
    for (auto& b : bodies_) if (b.alive) RecomputeInvInertiaWorld(b);

    const float kMatchR2 = 0.04f * 0.04f;  // 4cm point-match radius

    for (auto& c : contacts_) {
        Body& A = bodies_[c.a];
        Body& B = bodies_[c.b];
        c.rA = c.point - A.position;
        c.rB = c.point - B.position;
        ComputeBasis(c.normal, c.tangent1, c.tangent2);

        auto EffMass = [&](const glm::vec3& axis) {
            glm::vec3 rA_x_n = glm::cross(c.rA, axis);
            glm::vec3 rB_x_n = glm::cross(c.rB, axis);
            float m = A.inv_mass + B.inv_mass
                    + glm::dot(rA_x_n, A.inv_inertia_world * rA_x_n)
                    + glm::dot(rB_x_n, B.inv_inertia_world * rB_x_n);
            return m > 0.0f ? 1.0f / m : 0.0f;
        };

        c.normal_mass   = EffMass(c.normal);
        c.tangent_mass1 = EffMass(c.tangent1);
        c.tangent_mass2 = EffMass(c.tangent2);

        // Baumgarte positional bias.
        float pen = std::max(0.0f, c.penetration - kPenetrationSlop);
        c.bias = -(kBaumgarte / dt) * pen;

        // Restitution bias: closing velocity × restitution, only if significant.
        glm::vec3 rv = VelAtPoint(B, c.rB) - VelAtPoint(A, c.rA);
        float rv_n = glm::dot(rv, c.normal);
        float e = std::min(A.restitution, B.restitution);
        if (rv_n < -1.0f) c.bias += e * rv_n;

        // Warm-start: find a prior contact on the same pair at a nearby point.
        c.normal_impulse = 0.0f;
        c.tangent_impulse1 = 0.0f;
        c.tangent_impulse2 = 0.0f;
        for (const auto& pc : prev_contacts_) {
            if (pc.a != c.a || pc.b != c.b) continue;
            glm::vec3 d = pc.point - c.point;
            if (glm::dot(d, d) < kMatchR2) {
                c.normal_impulse   = pc.normal_impulse;
                c.tangent_impulse1 = pc.tangent_impulse1;
                c.tangent_impulse2 = pc.tangent_impulse2;
                break;
            }
        }
    }
}

void World::WarmStart() {
    for (auto& c : contacts_) {
        Body& A = bodies_[c.a];
        Body& B = bodies_[c.b];
        glm::vec3 P = c.normal * c.normal_impulse
                    + c.tangent1 * c.tangent_impulse1
                    + c.tangent2 * c.tangent_impulse2;
        A.linear_velocity  -= P * A.inv_mass;
        A.angular_velocity -= A.inv_inertia_world * glm::cross(c.rA, P);
        B.linear_velocity  += P * B.inv_mass;
        B.angular_velocity += B.inv_inertia_world * glm::cross(c.rB, P);
    }
}

void World::SolveVelocities() {
    for (auto& c : contacts_) {
        Body& A = bodies_[c.a];
        Body& B = bodies_[c.b];

        auto ApplyImpulse = [&](const glm::vec3& axis, float lambda) {
            glm::vec3 P = axis * lambda;
            A.linear_velocity  -= P * A.inv_mass;
            A.angular_velocity -= A.inv_inertia_world * glm::cross(c.rA, P);
            B.linear_velocity  += P * B.inv_mass;
            B.angular_velocity += B.inv_inertia_world * glm::cross(c.rB, P);
        };

        // Tangents first so friction is proportional to final normal impulse.
        {
            glm::vec3 rv = VelAtPoint(B, c.rB) - VelAtPoint(A, c.rA);
            float mu = std::sqrt(A.friction * B.friction);
            float max_fric = mu * c.normal_impulse;

            float rv_t1 = glm::dot(rv, c.tangent1);
            float lambda1 = -rv_t1 * c.tangent_mass1;
            float new_imp1 = std::clamp(c.tangent_impulse1 + lambda1, -max_fric, max_fric);
            lambda1 = new_imp1 - c.tangent_impulse1;
            c.tangent_impulse1 = new_imp1;
            ApplyImpulse(c.tangent1, lambda1);

            rv = VelAtPoint(B, c.rB) - VelAtPoint(A, c.rA);
            float rv_t2 = glm::dot(rv, c.tangent2);
            float lambda2 = -rv_t2 * c.tangent_mass2;
            float new_imp2 = std::clamp(c.tangent_impulse2 + lambda2, -max_fric, max_fric);
            lambda2 = new_imp2 - c.tangent_impulse2;
            c.tangent_impulse2 = new_imp2;
            ApplyImpulse(c.tangent2, lambda2);
        }

        // Normal impulse
        glm::vec3 rv = VelAtPoint(B, c.rB) - VelAtPoint(A, c.rA);
        float rv_n = glm::dot(rv, c.normal);
        float lambda = -(rv_n + c.bias) * c.normal_mass;
        float new_imp = std::max(0.0f, c.normal_impulse + lambda);
        lambda = new_imp - c.normal_impulse;
        c.normal_impulse = new_imp;
        ApplyImpulse(c.normal, lambda);
    }
}

void World::IntegrateVelocities(float dt) {
    for (auto& b : bodies_) {
        if (!b.alive || b.is_static || b.inv_mass == 0.0f) continue;
        if (b.sleeping) continue;
        b.position += b.linear_velocity * dt;
        glm::quat w(0.0f, b.angular_velocity.x, b.angular_velocity.y, b.angular_velocity.z);
        glm::quat dq = 0.5f * w * b.orientation;
        b.orientation = glm::normalize(b.orientation + dq * dt);
    }
}

void World::UpdateSleep(float dt) {
    for (auto& b : bodies_) {
        if (!b.alive || b.is_static || b.inv_mass == 0.0f) continue;
        float lin2 = glm::dot(b.linear_velocity, b.linear_velocity);
        float ang2 = glm::dot(b.angular_velocity, b.angular_velocity);
        if (lin2 < kSleepLinearSq && ang2 < kSleepAngularSq) {
            b.sleep_timer += dt;
            if (b.sleep_timer >= kSleepTime) {
                b.sleeping = true;
                b.linear_velocity = glm::vec3(0.0f);
                b.angular_velocity = glm::vec3(0.0f);
            }
        } else {
            b.sleep_timer = 0.0f;
            b.sleeping = false;
        }
    }
}

}  // namespace phys
