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

#include "entities/player.hpp"
#include "entities/world.hpp"
#include "systems/carry_system.hpp"
#include "systems/collision.hpp"
#include "systems/cpp_highlight_system.hpp"
#include "text/token.hpp"
#include "utils/config.hpp"
#include "word_ai_system.hpp"

namespace
Nimbin
{


static Vec3
perpendicularHorizontal(const Vec3& v)
{
    return { -v.z, 0.0, v.x };
}


void
WordAiSystem::update(DynArray<Text::TokenPtr>& words,
                     const Player& player,
                     CarriedLine& carried,
                     DynArray<Text::TokenPtr>& dropped,
                     double dt)
{
    DynArray<Text::TokenPtr> released;

    int active_ai_count = 0;
    for (const Text::TokenPtr& w : words) {
        if (!w) continue;
        if (w->ai.state != Text::Token::Ai::State::Idle) ++active_ai_count;
    }

    const size_t n = words.size();
    for (size_t i = 0; i < n; i++) {
        Text::TokenPtr& w = words[i];
        if (!w) continue;

        if ( w->ai.state  != Text::Token::Ai::State::Idle &&
             w->body.mode != PhysicsMode::AiHover &&
             w->body.mode != PhysicsMode::Hover) {
            for (Text::TokenPtr& w_stolen : w->ai.stolen) {
                if (!w_stolen) continue;
                w_stolen->body.mode = PhysicsMode::Dynamic;
                w_stolen->body.vel  = w->body.vel * 0.5;
                w_stolen->recolor();
                w_stolen->set_ColorSource(Text::ColorSource::Lint);
                released.push_back(std::move(w_stolen));
            }
            w->ai.stolen.clear();
            w->ai.state = Text::Token::Ai::State::Idle;
            w->recolor();
            w->set_ColorSource(Text::ColorSource::Lint);
            continue;
        }

        w->ai.state_timer += dt;

        switch (w->ai.state) {
            case Text::Token::Ai::State::Idle:    update_Idle   (*w, player, active_ai_count, dt); break;
            case Text::Token::Ai::State::Attack:  update_Attack (*w, player, carried, words, dropped, released, dt);
                                                                                                   break;
            case Text::Token::Ai::State::Flee:    update_Flee   (*w, player, released, dt);        break;
            case Text::Token::Ai::State::Stunned: update_Stunned(*w, released, dt);                break;
        }

        update_StolenTrail(*w, dt);
    }

    for (Text::TokenPtr& r : released)
        if (r) words.push_back(std::move(r));
}


void
WordAiSystem::update_Idle(Text::Token::Data& w, const Player& p, int active_ai_count, double dt)

{
    if (w.body.mode != PhysicsMode::Hover) return;
    if (active_ai_count >= _max_active_ai) return;

    Vec3 to_player = p.pos - w.pos;
    double dist_sq = to_player.x*to_player.x +
                     to_player.y*to_player.y +
                     to_player.z*to_player.z;

    if (dist_sq > Config::Ai::trigger_range * Config::Ai::trigger_range) return;

    double roll_threshold = Config::Ai::idle_aggro_roll * w.ai.aggressiveness * dt;
    if (Random::unit() > roll_threshold) return;

    if (Random::unit() < Config::Ai::flee_bias) enter_Flee(w);
    else                            enter_Attack(w);
}


void WordAiSystem::update_Attack(Text::Token::Data& w, const Player& p, CarriedLine& carried,
                                 DynArray<Text::TokenPtr>& all_words,
                                 DynArray<Text::TokenPtr>& dropped,
                                 DynArray<Text::TokenPtr>& released,
                                 double dt)
{
    const bool has_carry = (!carried.empty() && carried.last && carried.last->word);

    Vec3 target;
    if (has_carry) {
        target = carried.last->word->pos;
    }
    else {
        constexpr double ORBIT_RADIUS = 4.5;
        constexpr double ORBIT_SPEED  = 0.6;
        constexpr double ORBIT_HEIGHT = 0.5;

        w.ai.orbit_angle += w.ai.orbit_dir * ORBIT_SPEED * dt;

        target = {
            p.pos.x + ORBIT_RADIUS * SDL_cos(w.ai.orbit_angle),
            p.pos.y + ORBIT_HEIGHT,
            p.pos.z + ORBIT_RADIUS * SDL_sin(w.ai.orbit_angle)
        };
    }

    Vec3   to_target = target - w.pos;
    double dist      = Math::length(to_target);

    w.ai.phase_timer -= dt;
    if (w.ai.phase_timer <= 0.0) {
        w.ai.in_pause    = !w.ai.in_pause;
        w.ai.phase_timer = w.ai.in_pause
                           ? (0.4 + Random::unit() * 0.8)
                           : (1.0 + Random::unit() * 1.5);
    }

    Vec3 target_vel{};
    if (!w.ai.in_pause && dist > 0.01) {
        double inv = 1.0 / dist;
        Vec3   dir = { to_target.x * inv, to_target.y * inv, to_target.z * inv };
        target_vel = dir * w.ai.speed;

        w.ai.wiggle_phase += w.ai.wiggle_freq * dt;

        double wig = SDL_sin(w.ai.wiggle_phase) * Config::Ai::wiggle_amplitude;
        if (!has_carry) wig *= 0.4;

        Vec3 perp = perpendicularHorizontal(dir);

        target_vel.x += perp.x * wig;
        target_vel.z += perp.z * wig;
    }

    apply_Locomotion(w, target_vel, dt);

    if ( !carried.empty() &&
         !w.ai.has_stolen) {

        DynArray<CarriedWordNode*>
            nodes = carried.reverse_nodes();
        for (CarriedWordNode* node : nodes) {
            if (!node || !node->word) continue;
            if (!Collision::aabb_Overlap(w, *node->word)) continue;

            if (Random::unit() < Config::Ai::steal_probability) {
                Text::TokenPtr stolen = std::move(node->word);
                stolen->body.mode = PhysicsMode::AiHover;
                stolen->set_Color(Config::Color::attack);
                stolen->set_ColorSource(Text::ColorSource::Static);
                w.ai.stolen.push_back(std::move(stolen));
                enter_Flee(w);
                carried.compact();
                break;
            }  else {
                Text::TokenPtr knocked = std::move(node->word);
                knocked->body.mode = PhysicsMode::Dynamic;
                knocked->body.vel  = { 0.0, 2.0, 0.0 };
                dropped.push_back(std::move(knocked));
                enter_Flee(w);
            }
            carried.compact();
            break;
        }
    }

    for (Text::TokenPtr& other : all_words) {
        if (!other
            || (other.get()      == &w)
            || (other->ai.state  != Text::Token::Ai::State::Idle)
            || (other->body.mode != PhysicsMode::Hover)
            || (!Collision::aabb_Overlap(w, *other)))
            continue;

        Vec3   push_dir = other->pos - w.pos;

        double d = SDL_sqrt(push_dir.x*push_dir.x + push_dir.z*push_dir.z);
        if (d < 0.01) { push_dir = { 1.0, 0.0, 0.0 }; d = 1.0; }
        push_dir.x /= d;
        push_dir.z /= d;

        double kick_speed = SDL_max(Math::length(w.body.vel) * 1.5, 3.0);
        other->body.apply_KickImpulse({
                    push_dir.x * kick_speed, 1.5, push_dir.z * kick_speed
                });
    }

    Vec3 to_player = p.pos - w.pos;
    if (Math::length(to_player) > Config::Ai::trigger_range * 3.0) {
        release_StolenWords(w, released);
        enter_Stunned(w);
    }
}


void
WordAiSystem::update_Flee(Text::Token::Data& w, const Player& player,
                          DynArray<Text::TokenPtr>& released, double dt)
{
    Vec3 away = w.pos - player.pos;
    double dist = Math::length(away);

    w.ai.phase_timer -= dt;
    if (w.ai.phase_timer <= 0.0) {
        w.ai.in_pause    = !w.ai.in_pause;
        w.ai.phase_timer = w.ai.in_pause
                           ? (0.3 + Random::unit() * 0.5)
                           : (1.5 + Random::unit() * 1.5);
    }

    Vec3 target_vel{};
    if (!w.ai.in_pause && dist > 0.01) {
        double inv = 1.0 / dist;
        Vec3   dir = { away.x * inv, 0.0, away.z * inv };
        target_vel = dir * w.ai.speed * 1.2;

        w.ai.wiggle_phase += w.ai.wiggle_freq * dt;
        Vec3 perp = perpendicularHorizontal(dir);
        double wig = SDL_sin(w.ai.wiggle_phase) * Config::Ai::wiggle_amplitude * 0.6;
        target_vel.x += perp.x * wig;
        target_vel.z += perp.z * wig;
    }

    apply_Locomotion(w, target_vel, dt);

    if (dist > Config::Ai::flee_escape_dist) {
        if (w.ai.state_timer > Config::Ai::flee_escape_time) {
            if (!w.ai.stolen.empty()) {
                if (Random::unit() < Config::Ai::drop_loot_probability) {
                    release_StolenWords(w, released);
                }
                enter_Attack(w);
            }
            else {
                enter_Idle(w);
            }
        }
    }
}


void
WordAiSystem::update_Stunned(Text::Token::Data& w, DynArray<Text::TokenPtr>& released, double dt)
{
    apply_Locomotion(w, Vec3{}, dt);

    if (w.ai.state_timer > Config::Ai::stunned_duration) {
        release_StolenWords(w, released);
        enter_Idle(w);
    }
}

void
WordAiSystem::release_StolenWords(Text::Token::Data& w, DynArray<Text::TokenPtr>& out)
{
    for (Text::TokenPtr& s : w.ai.stolen) {
        if (!s) continue;
        s->body.mode = PhysicsMode::Dynamic;
        s->body.vel  = { w.body.vel.x * 0.5, 2.0, w.body.vel.z * 0.5 };
        s->recolor();
        s->set_ColorSource(Text::ColorSource::Lint);

        out.push_back(std::move(s));
    }
    w.ai.stolen.clear();
}


void
WordAiSystem::enter_Attack(Text::Token::Data& w)
{
    w.set_Color(Config::Color::attack);
    w.set_ColorSource(Text::ColorSource::Static);
    w.body.mode      = PhysicsMode::AiHover;
    w.ai.state       = Text::Token::Ai::State::Attack;
    w.ai.state_timer = 0.0;
    w.ai.phase_timer = 0.5 + Random::unit() * 1.5;
    w.ai.in_pause    = false;
    w.ai.has_stolen  = false;
    w.ai.hover_height_saved = w.body.hover_height;

    w.ai.orbit_angle = Random::unit() * SDL_PI_D * 2.0;
    w.ai.orbit_dir   = (Random::unit() < 0.5) ? -1.0 : 1.0;
}


void
WordAiSystem::enter_Flee(Text::Token::Data& w)
{
    w.set_Color(Config::Color::flee);
    w.set_ColorSource(Text::ColorSource::Static);
    w.body.mode      = PhysicsMode::AiHover;
    w.ai.state       = Text::Token::Ai::State::Flee;
    w.ai.state_timer = 0.0;
    w.ai.phase_timer = 0.3 + Random::unit() * 0.7;
    w.ai.in_pause    = false;
    w.ai.hover_height_saved = w.body.hover_height;
}


void
WordAiSystem::enter_Stunned(Text::Token::Data& w)
{
    w.ai.state       = Text::Token::Ai::State::Stunned;
    w.ai.state_timer = 0.0;
    w.body.vel       = Vec3{};
    w.set_Color(Config::Color::stunned);
    w.set_ColorSource(Text::ColorSource::Static);
}


void
WordAiSystem::enter_Idle(Text::Token::Data& w)
{
    w.ai.state       = Text::Token::Ai::State::Idle;
    w.ai.state_timer = 0.0;
    w.body.mode      = PhysicsMode::Hover;
    w.body.hover_height = w.ai.hover_height_saved;
    w.recolor();
    w.set_ColorSource(Text::ColorSource::Lint);
}


void
WordAiSystem::apply_Locomotion(Text::Token::Data& w, const Vec3& target_vel, double dt)
{
    double k = 1.0 - SDL_exp(-Config::Ai::velocity_lerp_rate * dt);
    w.body.vel.x += (target_vel.x - w.body.vel.x) * k;
    w.body.vel.y += (target_vel.y - w.body.vel.y) * k;
    w.body.vel.z += (target_vel.z - w.body.vel.z) * k;

    w.pos.x += w.body.vel.x * dt;
    w.pos.y += w.body.vel.y * dt;
    w.pos.z += w.body.vel.z * dt;

    const double ws = World::get_BoxScale<double>();

    auto clamp_axis = [](double& p, double& v, double he,
                         double lo, double hi) {
        if (p - he < lo) { p = lo + he; v = 0.0; }
        if (p + he > hi) { p = hi - he; v = 0.0; }
    };

    clamp_axis(w.pos.x, w.body.vel.x, w.shape.half_extents.x, -ws,       ws);
    clamp_axis(w.pos.y, w.body.vel.y, w.shape.half_extents.y, -ws + 0.5, ws);
    clamp_axis(w.pos.z, w.body.vel.z, w.shape.half_extents.z, -ws,       ws);
}


void
WordAiSystem::update_StolenTrail(Text::Token::Data& w, double dt)
{
    if (w.ai.stolen.empty()) return;

    // ── Smoothed back direction ─────────────────────────────────────
    double speed = SDL_sqrt(w.body.vel.x * w.body.vel.x +
                            w.body.vel.z * w.body.vel.z);

    if (speed > 0.5) {
        Vec3   raw_back = { -w.body.vel.x / speed, 0.0, -w.body.vel.z / speed };
        double k        = 1.0 - SDL_exp(-3.0 * dt);
        w.ai.trail_dir.x += (raw_back.x - w.ai.trail_dir.x) * k;
        w.ai.trail_dir.z += (raw_back.z - w.ai.trail_dir.z) * k;

        double len = SDL_sqrt(w.ai.trail_dir.x * w.ai.trail_dir.x +
                              w.ai.trail_dir.z * w.ai.trail_dir.z);
        if (len > 0.01) {
            w.ai.trail_dir.x /= len;
            w.ai.trail_dir.z /= len;
        }
    }

    if (SDL_fabs(w.ai.trail_dir.x) < 0.01 && SDL_fabs(w.ai.trail_dir.z) < 0.01)
        w.ai.trail_dir = { 1.0, 0.0, 0.0 };

    // ── Lerp stolen words toward their slot ─────────────────────────
    double offset     = static_cast<double>(w.half_extent);
    double pos_lerp_k = 1.0 - SDL_exp(-8.0 * dt);

    Vec3 perp = perpendicularHorizontal(w.ai.trail_dir);
    double target_yaw = SDL_atan2(-w.ai.trail_dir.x, -w.ai.trail_dir.z);

    for (Text::TokenPtr& s : w.ai.stolen) {
        if (!s) continue;
        offset += static_cast<double>(s->half_extent) + 0.18;

        // Per-link phase offsets so the chain doesn't sway in lockstep
        double bob_t  = w.body.hover_phase       + offset * 0.5;
        double sway_t = w.body.hover_phase * 1.3 + offset * 0.7;
        double sway   = SDL_sin(sway_t) * 0.12;

        Vec3 target {
            w.pos.x + w.ai.trail_dir.x * offset + perp.x * sway,
            w.pos.y + SDL_sin(bob_t) * 0.08,
            w.pos.z + w.ai.trail_dir.z * offset + perp.z * sway
        };

        s->pos.x += (target.x - s->pos.x) * pos_lerp_k;
        s->pos.y += (target.y - s->pos.y) * pos_lerp_k;
        s->pos.z += (target.z - s->pos.z) * pos_lerp_k;

        double yaw_diff = target_yaw - static_cast<double>(s->yaw);
        while (yaw_diff >  SDL_PI_D) yaw_diff -= 2.0 * SDL_PI_D;
        while (yaw_diff < -SDL_PI_D) yaw_diff += 2.0 * SDL_PI_D;
        s->yaw += static_cast<float>(yaw_diff * pos_lerp_k);

        offset += static_cast<double>(s->half_extent);
    }

    w.body.hover_phase += Config::Text3d::hover_speed * dt;
}

} // namespace Nimbin