#include <SDL3/SDL.h>
#include <algorithm>

#include "entities/game_object.hpp"
#include "systems/collision.hpp"

namespace
Nimbin
{

namespace Collision
{

static constexpr double FRICTION_IMPULSE_CUTOFF = 2.0;

bool
aabb_Overlap(const GameObject& a, const GameObject& b)
{
    return a.aabb_Min().x <= b.aabb_Max().x &&
           a.aabb_Max().x >= b.aabb_Min().x &&
           a.aabb_Min().y <= b.aabb_Max().y &&
           a.aabb_Max().y >= b.aabb_Min().y &&
           a.aabb_Min().z <= b.aabb_Max().z &&
           a.aabb_Max().z >= b.aabb_Min().z;
}


Manifold
aabb_Manifold(const GameObject& a, const GameObject& b)
{
    Manifold m;

    double overlap_x = SDL_min(a.aabb_Max().x, b.aabb_Max().x) -
                SDL_max(a.aabb_Min().x, b.aabb_Min().x);
    double overlap_y = SDL_min(a.aabb_Max().y, b.aabb_Max().y) -
                SDL_max(a.aabb_Min().y, b.aabb_Min().y);
    double overlap_z = SDL_min(a.aabb_Max().z, b.aabb_Max().z) -
                SDL_max(a.aabb_Min().z, b.aabb_Min().z);

    if (overlap_x <= 0.0 || overlap_y <= 0.0 || overlap_z <= 0.0)
        return m;

    m.hit = true;

    if (overlap_x < overlap_y && overlap_x < overlap_z) {
        m.overlap  = overlap_x;
        m.normal.x = (a.pos.x < b.pos.x) ? -1.0 : 1.0;
    }
    else if (overlap_y < overlap_z) {
        m.overlap  = overlap_y;
        m.normal.y = (a.pos.y < b.pos.y) ? -1.0 : 1.0;
    }
    else {
        m.overlap  = overlap_z;
        m.normal.z = (a.pos.z < b.pos.z) ? -1.0 : 1.0;
    }
    return m;
}


void
resolve_Objects(GameObject& a, GameObject& b)
{
    Manifold manifold = aabb_Manifold(a, b);
    if (!manifold.hit) return;

    const bool a_static = (a.body.mode == PhysicsMode::Static);
    const bool b_static = (b.body.mode == PhysicsMode::Static);

    const double share_a = b_static ? 1.0 : 0.5;
    const double share_b = a_static ? 1.0 : 0.5;
    if (!a_static) a.pos = a.pos + manifold.normal * (manifold.overlap * share_a);
    if (!b_static) b.pos = b.pos - manifold.normal * (manifold.overlap * share_b);

    // impulse resolution
    const double rel_vel = Math::dot(b.body.vel - a.body.vel, manifold.normal);
    if (rel_vel >= 0.0) return;

    const double inv_mass_a = a_static ? 0.0 : 1.0 / a.body.mass;
    const double inv_mass_b = b_static ? 0.0 : 1.0 / b.body.mass;
    const double e          = SDL_min(a.body.restitution, b.body.restitution);
    const double impulse    = -(1.0 + e) * rel_vel / (inv_mass_a + inv_mass_b);

    auto apply_impulse = [&](GameObject& o, double inv_mass, double sign) {
        if (inv_mass == 0.0) return;
        o.body.vel = o.body.vel + manifold.normal * (sign * impulse * inv_mass);

        if (SDL_fabs(impulse) < FRICTION_IMPULSE_CUTOFF) {
            const double vn  = Math::dot(o.body.vel, manifold.normal);
            Vec3         v_t = o.body.vel - manifold.normal * vn;
            v_t = v_t * o.body.friction;
            if (Math::length(v_t) < 0.02) v_t = Vec3{};
            o.body.vel = manifold.normal * vn + v_t;
        }
    };
    apply_impulse(a, inv_mass_a, +1.0);
    apply_impulse(b, inv_mass_b, -1.0);
}


Vec3
reflect(const Vec3& vel, const Vec3& normal, double restitution)
{
    double d = Math::dot(vel, normal);
    return {
        vel.x - (1.0 + restitution) * d * normal.x,
        vel.y - (1.0 + restitution) * d * normal.y,
        vel.z - (1.0 + restitution) * d * normal.z,
    };
}


SweptResult
swept_RayAabb(const Vec3& origin, const Vec3& dir,
              const Vec3& box_min, const Vec3& box_max)
{
    double t_enter   = -1e18;   // latest entry time across slabs (real, unclamped)
    double t_exit    =  1e18;   // earliest exit time across slabs
    int    axis_in   = -1;      // which axis produced t_enter
    double sign_in   =  0.0;    // sign overlap_f the entry-face normal overlap_n that axis

    auto slab = [&](double o, double d, double lo, double hi, int ax) -> bool
    {
        if (SDL_fabs(d) < 1e-9) {
            // Movement parallel to slab. Miss only if origin is outside slab.
            return (o >= lo && o <= hi);
        }
        double inv  = 1.0 / d;
        double t1   = (lo - o) * inv;    // enters slab from `-` face if d>0
        double t2   = (hi - o) * inv;    // exits slab through `+` face if d>0
        double sign = -1.0;              // assume entry through `-` face
        if (t1 > t2) {                   // moving in -axis direction
            double tmp = t1; t1 = t2; t2 = tmp;
            sign = 1.0;                  // entry through `+` face instead
        }
        if (t1 > t_enter) { t_enter = t1; axis_in = ax; sign_in = sign; }
        if (t2 < t_exit)  { t_exit  = t2; }
        return true;
    };

    if (!slab(origin.x, dir.x, box_min.x, box_max.x, 0)) return {};
    if (!slab(origin.y, dir.y, box_min.y, box_max.y, 1)) return {};
    if (!slab(origin.z, dir.z, box_min.z, box_max.z, 2)) return {};

    // Miss if the slabs don't overlap, overlap_r the hit window is outside [0, 1]
    if (t_enter > t_exit) return {};
    if (t_exit  < 0.0)    return {};   // already past the box
    if (t_enter > 1.0)    return {};   // hit is in a future step

    SweptResult r;
    r.hit = true;
    r.t   = SDL_max(t_enter, 0.0);     // clamp to this step
    if (axis_in == 0) r.normal.x = sign_in;
    else if (axis_in == 1) r.normal.y = sign_in;
    else if (axis_in == 2) r.normal.z = sign_in;
    return r;
}

}

} // namespace Nimbin