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

#include "entities/world.hpp"
#include "systems/collision.hpp"
#include "systems/physics_system.hpp"
#include "systems/projectile_system.hpp"
#include "text/token.hpp"
#include "utils/config.hpp"
#include "utils/frame_profiler.hpp"
#include "wall/label.hpp"

namespace
Nimbin
{

void
PhysicsSystem::update_Player(Player& p, double acc_x, double acc_z, double dt)
{
    dt = SDL_min(dt, 1.0 / 30.0);

    constexpr double rate = 6.0;
    constexpr double grav = 25.0;

    double drag = SDL_exp(-dt * rate);
    double diff = 1.0 - drag;

    double vx = p.vel.x;
    double vy = p.vel.y;
    double vz = p.vel.z;

    p.vel.x -= vx * diff;
    p.vel.y -= grav * dt;
    p.vel.z -= vz * diff;

    p.vel.x += diff * acc_x / rate;
    p.vel.z += diff * acc_z / rate;

    p.pos.x += (dt - diff / rate) * acc_x / rate + diff * vx / rate;
    p.pos.y += -0.5 * grav * dt * dt + vy * dt;
    p.pos.z += (dt - diff / rate) * acc_z / rate + diff * vz / rate;
}


void
PhysicsSystem::resolve_PlayerVsObjects(Player& player, CarriedLine& carried,
                                       const DynArray<GameObject*>& colliders)
{
    const double px_min = player.pos.x - static_cast<double>(player.radius);
    const double px_max = player.pos.x + static_cast<double>(player.radius);
    const double py_min = player.pos.y - static_cast<double>(player.height);
    const double py_max = player.pos.y;
    const double pz_min = player.pos.z - static_cast<double>(player.radius);
    const double pz_max = player.pos.z + static_cast<double>(player.radius);

    DynArray<CarriedWordNode*> nodes = carried.nodes();

    for (GameObject* obj : colliders) {
        if (!obj || !obj->solid) continue;

        const Vec3 omin = obj->aabb_Min();
        const Vec3 omax = obj->aabb_Max();

        if (px_max >= omin.x && px_min <= omax.x &&
            py_max >= omin.y && py_min <= omax.y &&
            pz_max >= omin.z && pz_min <= omax.z) {
            double ox = SDL_min(px_max, omax.x) - SDL_max(px_min, omin.x);
            double oy = SDL_min(py_max, omax.y) - SDL_max(py_min, omin.y);
            double oz = SDL_min(pz_max, omax.z) - SDL_max(pz_min, omin.z);

            Vec3   normal{};
            double overlap{};

            if (ox < oy && ox < oz) {
                overlap  = ox;
                normal.x = (player.pos.x < obj->pos.x) ? -1.0 : 1.0;
            }
            else if (oy < oz) {
                overlap  = oy;
                normal.y = (player.pos.y < obj->pos.y) ? -1.0 : 1.0;
            }
            else {
                overlap  = oz;
                normal.z = (player.pos.z < obj->pos.z) ? -1.0 : 1.0;
            }

            if (normal.y > 0.0)
                player.on_ground = true;

            player.pos.x += normal.x * overlap * 0.1;
            player.pos.y += normal.y * overlap * 0.1;
            player.pos.z += normal.z * overlap * 0.1;

            double vel_along = Math::dot(player.vel, normal);
            if (vel_along < 0.0) {
                player.vel.x -= vel_along * normal.x * 0.1;
                player.vel.y -= vel_along * normal.y * 0.1;
                player.vel.z -= vel_along * normal.z * 0.1;
            }

            double speed = SDL_sqrt(player.vel.x * player.vel.x +
                                    player.vel.z * player.vel.z);
            double kick  = SDL_max(speed * 2.0, 4.0);
            if (obj->body.mode != PhysicsMode::AiHover && normal.y <= 0.0)
                obj->body.apply_KickImpulse({ -normal.x * kick, 0.3, -normal.z * kick });
        }

        {
            constexpr double GROUND_EPS = 0.1;
            const bool xz_overlap = (px_max > omin.x && px_min < omax.x &&
                                     pz_max > omin.z && pz_min < omax.z);
            const bool feet_on_top = (py_min <= omax.y + GROUND_EPS &&
                                      py_min >= omax.y - GROUND_EPS);
            if (xz_overlap && feet_on_top && player.vel.y <= 0.01)
                player.on_ground = true;
        }

        for (CarriedWordNode* node : nodes) {
            if (!node || !node->word)
                        continue;

            GameObject& w = *node->word;
            if (!Collision::aabb_Overlap(w, *obj))
                        continue;

            Collision::Manifold m = Collision::aabb_Manifold(w, *obj);
            if (!m.hit) continue;

            Vec3 normal  = m.normal;
            double overlap = m.overlap;

            w.pos.x += normal.x * overlap;
            w.pos.y += normal.y * overlap;
            w.pos.z += normal.z * overlap;

            double speed = SDL_sqrt(player.vel.x * player.vel.x +
                                    player.vel.z * player.vel.z);

            double kick = SDL_max(speed * 2.0, 4.0);
            if (obj->body.mode != PhysicsMode::AiHover)
                obj->body.apply_KickImpulse({ -normal.x * kick, 0.3, -normal.z * kick });
        }
    }
}


void
PhysicsSystem::update_Objects(const DynArray<GameObject*>& objects,
                              double dt, double gravity)
{
    { FP_ZONE("phys.integrate");
        for (GameObject* obj : objects) {
            if (!obj) continue;

            if (obj->body.mode == PhysicsMode::AiHover)
                continue;

            if (obj->body.mode == PhysicsMode::Hover) {
                obj->body.hover_phase += Config::Text3d::hover_speed * dt;
                obj->pos.y = obj->body.hover_height +
                             SDL_sin(obj->body.hover_phase) * Config::Text3d::hover_amp;
                continue;
            }

            if (obj->body.mode != PhysicsMode::Dynamic)
                continue;

            obj->body.apply_Force({ 0.0, -gravity * obj->body.mass, 0.0 });
            obj->body.integrate(dt);
            obj->pos.x += obj->body.vel.x * dt;
            obj->pos.y += obj->body.vel.y * dt;
            obj->pos.z += obj->body.vel.z * dt;
        }
    }

    { FP_ZONE("phys.bounds");
        for (GameObject* obj : objects) {
            if (!obj || obj->body.mode != PhysicsMode::Dynamic)
                continue;
            resolve_WorldBounds(*obj);
        }
    }

    { FP_ZONE("phys.pairs");
        _grid.build(objects);

        DynArray<GameObject*> active;
        active.reserve(objects.size());
        for (GameObject* o : objects) {
            if (!o)
                continue;
            const PhysicsMode m = o->body.mode;
            if (m == PhysicsMode::Dynamic || m == PhysicsMode::Hover)
                active.push_back(o);
        }

        collide_Grid(active);
    }
}


void
PhysicsSystem::try_Pair(GameObject* a, GameObject* b)
{
    if (!a || !b) return;

    PhysicsMode mi = a->body.mode;
    PhysicsMode mj = b->body.mode;

    bool i_dyn = (mi == PhysicsMode::Dynamic);
    bool j_dyn = (mj == PhysicsMode::Dynamic);
    bool i_hov = (mi == PhysicsMode::Hover);
    bool j_hov = (mj == PhysicsMode::Hover);
    bool i_static_solid = (mi == PhysicsMode::Static && a->solid);
    bool j_static_solid = (mj == PhysicsMode::Static && b->solid);

    if (!i_dyn && !j_dyn && !i_hov && !j_hov) return;
    if (!Collision::aabb_Overlap(*a, *b))     return;

    if (i_dyn && j_hov) {
        b->body.mode = PhysicsMode::Dynamic;
        b->body.vel  = a->body.vel * 0.5;
    }
    else if (j_dyn && i_hov) {
        a->body.mode = PhysicsMode::Dynamic;
        a->body.vel  = b->body.vel * 0.5;
    }

    bool i_dyn_now = (a->body.mode == PhysicsMode::Dynamic);
    bool j_dyn_now = (b->body.mode == PhysicsMode::Dynamic);

    if ( (i_dyn_now && j_dyn_now) ||
         (i_dyn_now && j_static_solid) ||
         (j_dyn_now && i_static_solid) ) {
        Collision::resolve_Objects(*a, *b);
    }
}


void
PhysicsSystem::resolve_WorldBounds(GameObject& obj)
{
    const double ws = World::get_BoxScale<double>();
    Vec3&        he = obj.shape.half_extents;

    // Floor
    if (obj.pos.y - he.y < -ws) {
        obj.pos.y      = -ws + he.y;
        obj.body.vel.y = SDL_fabs(obj.body.vel.y) * obj.body.restitution;
        obj.body.vel.x *= obj.body.friction;
        obj.body.vel.z *= obj.body.friction;
        if (SDL_fabs(obj.body.vel.y) < 0.05) obj.body.vel.y = 0.0;
        if (SDL_fabs(obj.body.vel.x) < 0.01) obj.body.vel.x = 0.0;
        if (SDL_fabs(obj.body.vel.z) < 0.01) obj.body.vel.z = 0.0;
    }
    // Ceiling
    if (obj.pos.y + he.y > ws) {
        obj.pos.y      =  ws - he.y;
        obj.body.vel.y = -SDL_fabs(obj.body.vel.y) * obj.body.restitution;
    }
    // Side walls
    if (obj.pos.x + he.x >  ws) { obj.pos.x =  ws - he.x; obj.body.vel.x *= -obj.body.restitution; }
    if (obj.pos.x - he.x < -ws) { obj.pos.x = -ws + he.x; obj.body.vel.x *= -obj.body.restitution; }
    if (obj.pos.z + he.z >  ws) { obj.pos.z =  ws - he.z; obj.body.vel.z *= -obj.body.restitution; }
    if (obj.pos.z - he.z < -ws) { obj.pos.z = -ws + he.z; obj.body.vel.z *= -obj.body.restitution; }
}


void
PhysicsSystem::update_Projectiles(DynArray<Projectile>& projectiles, double dt,
                                  const DynArray<Text::TokenPtr>& words)
{
    for (Projectile& p : projectiles) {
        if (p.sticking)      continue;
        if (p.words.empty()) continue;

        p.step_dt       = dt;
        p.prev_centroid = Text::Token::Data::words_centroid(p.words);

        if (p.track_obj) {
            bool alive = false;
            for (const Text::TokenPtr& w : words)
                if (w.get() == p.track_obj) { alive = true; break; }

            if (alive) {
                Text::Token::Data* o  = p.track_obj;
                const double cy = SDL_cos(o->yaw), sy = SDL_sin(o->yaw);
                const Vec3 live {
                    o->pos.x + cy * p.track_local.x + sy * p.track_local.z,
                    o->pos.y + p.track_local.y,
                    o->pos.z - sy * p.track_local.x + cy * p.track_local.z
                };
                p.track_last = live;

                const Vec3 c = p.prev_centroid;
                Vec3 d { live.x - c.x, live.y - c.y, live.z - c.z };

                const double L = Math::length(d);
                if (L > 1e-6) {
                    const double s = Config::Physics::proj_throw_speed / L;
                    p.vel = Vec3{ d.x * s, d.y * s, d.z * s };
                }
            }
            else {
                p.track_obj = nullptr;
                p.vel.y -= Config::Physics::proj_grav * dt;
            }
        }
        else {
            p.vel.y -= Config::Physics::proj_grav * dt;
        }

        for (Text::TokenPtr& w : p.words) {
            if (!w) continue;
            w->pos.x += p.vel.x * dt;
            w->pos.y += p.vel.y * dt;
            w->pos.z += p.vel.z * dt;
        }
    }
}


bool
PhysicsSystem::resolve_SingleBoxCollision(
        Projectile& proj,
        const Vec3& box_min_raw, const Vec3& box_max_raw,
        const Vec3& word_he, double restitution,
        Vec3& cur, Vec3& prev, Vec3& dir,
        Collision::SweptResult& out_sw)
{
    constexpr double EPS = 0.001;

    // Minkowski-expanded AABB
    Vec3 box_min { box_min_raw.x - word_he.x,
                   box_min_raw.y - word_he.y,
                   box_min_raw.z - word_he.z };
    Vec3 box_max { box_max_raw.x + word_he.x,
                   box_max_raw.y + word_he.y,
                   box_max_raw.z + word_he.z };

    out_sw = Collision::swept_RayAabb(prev, dir, box_min, box_max);
    if (!out_sw.hit) return false;

    Vec3 contact {
        prev.x + dir.x * out_sw.t,
        prev.y + dir.y * out_sw.t,
        prev.z + dir.z * out_sw.t
    };

    if      (out_sw.normal.x < 0) contact.x = box_min.x - EPS;
    else if (out_sw.normal.x > 0) contact.x = box_max.x + EPS;
    if      (out_sw.normal.y < 0) contact.y = box_min.y - EPS;
    else if (out_sw.normal.y > 0) contact.y = box_max.y + EPS;
    if      (out_sw.normal.z < 0) contact.z = box_min.z - EPS;
    else if (out_sw.normal.z > 0) contact.z = box_max.z + EPS;

    double push_x = contact.x - cur.x;
    double push_y = contact.y - cur.y;
    double push_z = contact.z - cur.z;
    for (const Text::TokenPtr& gw : proj.words) {
        if (!gw) continue;
        gw->pos.x += push_x;
        gw->pos.y += push_y;
        gw->pos.z += push_z;
    }

    proj.vel = Collision::reflect(proj.vel, out_sw.normal, restitution);

    cur  = contact;
    prev = contact;
    dir  = {};

    return true;
}


void
PhysicsSystem::resolve_ProjectileStatics(Projectile& proj,
                                         const DynArray<GameObject*>& statics,
                                         Vec3& cur, Vec3& prev, Vec3& dir,
                                         const Vec3& word_he,
                                         DynArray<ProjectileContactEvent>* contacts)
{
    constexpr double RESTITUTION_STATIC = 0.45;
    constexpr double RESTITUTION_TEXT   = 0.55;

    for (GameObject* obj : statics) {
        if (!obj || !obj->solid) continue;

        const bool   is_text     = (obj->type == ObjectType::Text);
        const double restitution = is_text ? RESTITUTION_TEXT : RESTITUTION_STATIC;
        const Vec3   vel_before  = proj.vel;

        Collision::SweptResult sw;
        if (!resolve_SingleBoxCollision(proj, obj->aabb_Min(), obj->aabb_Max(),
                                        word_he, restitution, cur, prev, dir, sw))
            continue;

        proj.collided = true;

        if (is_text) {
            if (obj == proj.track_obj) proj.track_obj = nullptr;
            obj->body.apply_KickImpulse({
                    vel_before.x * 0.4,
                    SDL_fabs(vel_before.y) * 0.1 + 0.3,
                    vel_before.z * 0.4
                });
        }

        if (contacts) {
            contacts->push_back(ProjectileContactEvent{
                                    .point      = cur,
                                    .normal     = sw.normal,
                                    .other      = obj,
                                    .projectile = &proj,
                                });
        }
    }
}


void
PhysicsSystem::resolve_ProjectileLabels(Projectile& proj,
                                        const DynArray<Text::Handler*>& labels,
                                        Vec3& cur, Vec3& prev, Vec3& dir,
                                        const Vec3& word_he)
{
    constexpr double RESTITUTION_LABEL = 0.55;
    for (Text::Handler* label : labels) {
        if (!label || label->is_Empty()) continue;
        const auto& lw = label->get_Words();
        for (size_t i = 0; i < lw.size(); i++) {
            const Text::TokenPtr& hw = lw[i];
            if (!hw || !hw->loaded) continue;
            Vec3 vel_before = proj.vel;
            Collision::SweptResult sw;
            if (resolve_SingleBoxCollision(proj, hw->aabb_Min(), hw->aabb_Max(),
                                           word_he, RESTITUTION_LABEL,
                                           cur, prev, dir, sw)) {
                proj.collided = true;
                label->kick(i, vel_before, sw.normal);
            }
        }
    }
}


void
PhysicsSystem::resolve_Projectiles(DynArray<Projectile>& projectiles,
                                   const DynArray<GameObject*>* statics,
                                   const DynArray<Text::Handler*>*  labels,
                                   DynArray<ProjectileContactEvent>* contacts)
{
    for (Projectile& proj : projectiles) {
        if (proj.sticking || proj.words.empty()) continue;

        Vec3 cur  = Text::Token::Data::words_centroid(proj.words);
        Vec3 prev = proj.prev_centroid;
        Vec3 dir  { cur.x - prev.x, cur.y - prev.y, cur.z - prev.z };

        Vec3 word_he { 0.3, 0.07, 0.07 };
        for (const Text::TokenPtr& w : proj.words) {
            if (w) { word_he = w->shape.half_extents; break; }
        }

        if (statics) resolve_ProjectileStatics(proj, *statics, cur, prev, dir, word_he, contacts);
        if (labels)  resolve_ProjectileLabels (proj, *labels,  cur, prev, dir, word_he);
    }
}


void
PhysicsSystem::SpatialGrid::build(const DynArray<GameObject*>& objs)
{
    cell = 2.0;
    inv  = 1.0 / cell;
    min_x = min_z = -World::get_BoxScale<double>();
    const double span = World::get_BoxScale<double>() * 2.0;
    nx = nz = static_cast<int>(span * inv) + 1;

    const int total = nx * nz;
    if (static_cast<int>(cells.size()) < total) cells.resize(static_cast<size_t>(total));
    for (auto& c : cells) c.clear();

    for (GameObject* o : objs) {
        if (!o) continue;
        int cx = static_cast<int>((o->pos.x - min_x) * inv);
        int cz = static_cast<int>((o->pos.z - min_z) * inv);
        cx = cx < 0 ? 0 : (cx >= nx ? nx - 1 : cx);
        cz = cz < 0 ? 0 : (cz >= nz ? nz - 1 : cz);
        cells[static_cast<size_t>(index(cx, cz))].push_back(o);
    }
}


void
PhysicsSystem::collide_Grid(const DynArray<GameObject*>& active)
{
    for (GameObject* a : active) {
        if (!a) continue;

        int cx = static_cast<int>((a->pos.x - _grid.min_x) * _grid.inv);
        int cz = static_cast<int>((a->pos.z - _grid.min_z) * _grid.inv);
        cx = cx < 0 ? 0 : (cx >= _grid.nx ? _grid.nx - 1 : cx);
        cz = cz < 0 ? 0 : (cz >= _grid.nz ? _grid.nz - 1 : cz);

        for (int dz = -1; dz <= 1; ++dz) {
            const int ncz = cz + dz;
            if (ncz < 0 || ncz >= _grid.nz) continue;

            for (int dx = -1; dx <= 1; ++dx) {
                const int ncx = cx + dx;
                if (ncx < 0 || ncx >= _grid.nx) continue;

                const auto& cell = _grid.cells[static_cast<size_t>(_grid.index(ncx, ncz))];
                for (GameObject* b : cell) {
                    if (b == a) continue;

                    const PhysicsMode mb = b->body.mode;
                    const bool b_active = (mb == PhysicsMode::Dynamic ||
                                           mb == PhysicsMode::Hover);
                    if (b_active && b < a) continue;

                    try_Pair(a, b);
                }
            }
        }
    }
}

} // namespace Nimbin