vibe3_physics/source/boids_mgr/boid_manager.cpp

#include "boid_manager.h"

#include <algorithm>  // for std::min, std::max
#include <cmath>      // for sqrt, atan2

#include "../ball.h"                // for Ball
#include "../engine.h"              // for Engine (si se necesita)
#include "../scene/scene_manager.h" // for SceneManager
#include "../state/state_manager.h" // for StateManager
#include "../ui/ui_manager.h"       // for UIManager

BoidManager::BoidManager()
    : engine_(nullptr)
    , scene_mgr_(nullptr)
    , ui_mgr_(nullptr)
    , state_mgr_(nullptr)
    , screen_width_(0)
    , screen_height_(0)
    , boids_active_(false)
    , spatial_grid_(800, 600, BOID_GRID_CELL_SIZE) {  // Tamaño por defecto, se actualiza en initialize()
}

BoidManager::~BoidManager() {
}

void BoidManager::initialize(Engine* engine, SceneManager* scene_mgr, UIManager* ui_mgr,
                              StateManager* state_mgr, int screen_width, int screen_height) {
    engine_ = engine;
    scene_mgr_ = scene_mgr;
    ui_mgr_ = ui_mgr;
    state_mgr_ = state_mgr;
    screen_width_ = screen_width;
    screen_height_ = screen_height;

    // Actualizar dimensiones del spatial grid
    spatial_grid_.updateWorldSize(screen_width, screen_height);
}

void BoidManager::updateScreenSize(int width, int height) {
    screen_width_ = width;
    screen_height_ = height;

    // Actualizar dimensiones del spatial grid (FASE 2)
    spatial_grid_.updateWorldSize(width, height);
}

void BoidManager::activateBoids() {
    boids_active_ = true;

    // Desactivar gravedad al entrar en modo boids
    scene_mgr_->forceBallsGravityOff();

    // Inicializar velocidades aleatorias para los boids
    auto& balls = scene_mgr_->getBallsMutable();
    for (auto& ball : balls) {
        // Dar velocidad inicial aleatoria si está quieto
        float vx, vy;
        ball->getVelocity(vx, vy);
        if (vx == 0.0f && vy == 0.0f) {
            // Velocidad aleatoria entre -1 y 1
            vx = (rand() % 200 - 100) / 100.0f;
            vy = (rand() % 200 - 100) / 100.0f;
            ball->setVelocity(vx, vy);
        }
    }

    // Mostrar notificación (solo si NO estamos en modo demo o logo)
    if (state_mgr_ && ui_mgr_ && state_mgr_->getCurrentMode() == AppMode::SANDBOX) {
        ui_mgr_->showNotification("Modo Boids");
    }
}

void BoidManager::deactivateBoids(bool force_gravity_on) {
    if (!boids_active_) return;

    boids_active_ = false;

    // Activar gravedad al salir (si se especifica)
    if (force_gravity_on) {
        scene_mgr_->forceBallsGravityOn();
    }

    // Mostrar notificación (solo si NO estamos en modo demo o logo)
    if (state_mgr_ && ui_mgr_ && state_mgr_->getCurrentMode() == AppMode::SANDBOX) {
        ui_mgr_->showNotification("Modo Física");
    }
}

void BoidManager::toggleBoidsMode(bool force_gravity_on) {
    if (boids_active_) {
        deactivateBoids(force_gravity_on);
    } else {
        activateBoids();
    }
}

void BoidManager::update(float delta_time) {
    if (!boids_active_) return;

    auto& balls = scene_mgr_->getBallsMutable();

    // FASE 2: Poblar spatial grid al inicio de cada frame (O(n))
    spatial_grid_.clear();
    for (auto& ball : balls) {
        SDL_FRect pos = ball->getPosition();
        float center_x = pos.x + pos.w / 2.0f;
        float center_y = pos.y + pos.h / 2.0f;
        spatial_grid_.insert(ball.get(), center_x, center_y);
    }

    // Aplicar las tres reglas de Reynolds a cada boid
    // FASE 2: Ahora usa spatial grid para búsquedas O(1) en lugar de O(n)
    for (auto& ball : balls) {
        applySeparation(ball.get(), delta_time);
        applyAlignment(ball.get(), delta_time);
        applyCohesion(ball.get(), delta_time);
        applyBoundaries(ball.get());
        limitSpeed(ball.get());
    }

    // Actualizar posiciones con velocidades resultantes
    for (auto& ball : balls) {
        float vx, vy;
        ball->getVelocity(vx, vy);

        SDL_FRect pos = ball->getPosition();
        pos.x += vx;
        pos.y += vy;

        ball->setPosition(pos.x, pos.y);
    }
}

// ============================================================================
// REGLAS DE REYNOLDS (1987)
// ============================================================================

void BoidManager::applySeparation(Ball* boid, float delta_time) {
    // Regla 1: Separación - Evitar colisiones con vecinos cercanos
    float steer_x = 0.0f;
    float steer_y = 0.0f;
    int count = 0;

    SDL_FRect pos = boid->getPosition();
    float center_x = pos.x + pos.w / 2.0f;
    float center_y = pos.y + pos.h / 2.0f;

    // FASE 2: Usar spatial grid para buscar solo vecinos cercanos (O(1) en lugar de O(n))
    auto neighbors = spatial_grid_.queryRadius(center_x, center_y, BOID_SEPARATION_RADIUS);

    for (Ball* other : neighbors) {
        if (other == boid) continue;  // Ignorar a sí mismo

        SDL_FRect other_pos = other->getPosition();
        float other_x = other_pos.x + other_pos.w / 2.0f;
        float other_y = other_pos.y + other_pos.h / 2.0f;

        float dx = center_x - other_x;
        float dy = center_y - other_y;
        float distance = std::sqrt(dx * dx + dy * dy);

        if (distance > 0.0f && distance < BOID_SEPARATION_RADIUS) {
            // FASE 1.3: Separación más fuerte cuando más cerca (inversamente proporcional a distancia)
            // Fuerza proporcional a cercanía: 0% en radio máximo, 100% en colisión
            float separation_strength = (BOID_SEPARATION_RADIUS - distance) / BOID_SEPARATION_RADIUS;
            steer_x += (dx / distance) * separation_strength;
            steer_y += (dy / distance) * separation_strength;
            count++;
        }
    }

    if (count > 0) {
        // Promedio
        steer_x /= count;
        steer_y /= count;

        // Aplicar fuerza de separación
        float vx, vy;
        boid->getVelocity(vx, vy);
        vx += steer_x * BOID_SEPARATION_WEIGHT * delta_time;
        vy += steer_y * BOID_SEPARATION_WEIGHT * delta_time;
        boid->setVelocity(vx, vy);
    }
}

void BoidManager::applyAlignment(Ball* boid, float delta_time) {
    // Regla 2: Alineación - Seguir dirección promedio del grupo
    float avg_vx = 0.0f;
    float avg_vy = 0.0f;
    int count = 0;

    SDL_FRect pos = boid->getPosition();
    float center_x = pos.x + pos.w / 2.0f;
    float center_y = pos.y + pos.h / 2.0f;

    // FASE 2: Usar spatial grid para buscar solo vecinos cercanos (O(1) en lugar de O(n))
    auto neighbors = spatial_grid_.queryRadius(center_x, center_y, BOID_ALIGNMENT_RADIUS);

    for (Ball* other : neighbors) {
        if (other == boid) continue;

        SDL_FRect other_pos = other->getPosition();
        float other_x = other_pos.x + other_pos.w / 2.0f;
        float other_y = other_pos.y + other_pos.h / 2.0f;

        float dx = center_x - other_x;
        float dy = center_y - other_y;
        float distance = std::sqrt(dx * dx + dy * dy);

        if (distance < BOID_ALIGNMENT_RADIUS) {
            float other_vx, other_vy;
            other->getVelocity(other_vx, other_vy);
            avg_vx += other_vx;
            avg_vy += other_vy;
            count++;
        }
    }

    if (count > 0) {
        // Velocidad promedio del grupo
        avg_vx /= count;
        avg_vy /= count;

        // Steering hacia la velocidad promedio
        float vx, vy;
        boid->getVelocity(vx, vy);
        float steer_x = (avg_vx - vx) * BOID_ALIGNMENT_WEIGHT * delta_time;
        float steer_y = (avg_vy - vy) * BOID_ALIGNMENT_WEIGHT * delta_time;

        // Limitar fuerza máxima de steering
        float steer_mag = std::sqrt(steer_x * steer_x + steer_y * steer_y);
        if (steer_mag > BOID_MAX_FORCE) {
            steer_x = (steer_x / steer_mag) * BOID_MAX_FORCE;
            steer_y = (steer_y / steer_mag) * BOID_MAX_FORCE;
        }

        vx += steer_x;
        vy += steer_y;
        boid->setVelocity(vx, vy);
    }
}

void BoidManager::applyCohesion(Ball* boid, float delta_time) {
    // Regla 3: Cohesión - Moverse hacia el centro de masa del grupo
    float center_of_mass_x = 0.0f;
    float center_of_mass_y = 0.0f;
    int count = 0;

    SDL_FRect pos = boid->getPosition();
    float center_x = pos.x + pos.w / 2.0f;
    float center_y = pos.y + pos.h / 2.0f;

    // FASE 2: Usar spatial grid para buscar solo vecinos cercanos (O(1) en lugar de O(n))
    auto neighbors = spatial_grid_.queryRadius(center_x, center_y, BOID_COHESION_RADIUS);

    for (Ball* other : neighbors) {
        if (other == boid) continue;

        SDL_FRect other_pos = other->getPosition();
        float other_x = other_pos.x + other_pos.w / 2.0f;
        float other_y = other_pos.y + other_pos.h / 2.0f;

        float dx = center_x - other_x;
        float dy = center_y - other_y;
        float distance = std::sqrt(dx * dx + dy * dy);

        if (distance < BOID_COHESION_RADIUS) {
            center_of_mass_x += other_x;
            center_of_mass_y += other_y;
            count++;
        }
    }

    if (count > 0) {
        // Centro de masa del grupo
        center_of_mass_x /= count;
        center_of_mass_y /= count;

        // FASE 1.4: Normalizar dirección hacia el centro (CRÍTICO - antes no estaba normalizado!)
        float dx_to_center = center_of_mass_x - center_x;
        float dy_to_center = center_of_mass_y - center_y;
        float distance_to_center = std::sqrt(dx_to_center * dx_to_center + dy_to_center * dy_to_center);

        // Solo aplicar si hay distancia al centro (evitar división por cero)
        if (distance_to_center > 0.1f) {
            // Normalizar vector dirección (fuerza independiente de distancia)
            float steer_x = (dx_to_center / distance_to_center) * BOID_COHESION_WEIGHT * delta_time;
            float steer_y = (dy_to_center / distance_to_center) * BOID_COHESION_WEIGHT * delta_time;

            // Limitar fuerza máxima de steering
            float steer_mag = std::sqrt(steer_x * steer_x + steer_y * steer_y);
            if (steer_mag > BOID_MAX_FORCE) {
                steer_x = (steer_x / steer_mag) * BOID_MAX_FORCE;
                steer_y = (steer_y / steer_mag) * BOID_MAX_FORCE;
            }

            float vx, vy;
            boid->getVelocity(vx, vy);
            vx += steer_x;
            vy += steer_y;
            boid->setVelocity(vx, vy);
        }
    }
}

void BoidManager::applyBoundaries(Ball* boid) {
    // Mantener boids dentro de los límites de la pantalla
    // Comportamiento "wrapping" (teletransporte al otro lado)
    SDL_FRect pos = boid->getPosition();
    float center_x = pos.x + pos.w / 2.0f;
    float center_y = pos.y + pos.h / 2.0f;

    bool wrapped = false;

    if (center_x < 0) {
        pos.x = screen_width_ - pos.w / 2.0f;
        wrapped = true;
    } else if (center_x > screen_width_) {
        pos.x = -pos.w / 2.0f;
        wrapped = true;
    }

    if (center_y < 0) {
        pos.y = screen_height_ - pos.h / 2.0f;
        wrapped = true;
    } else if (center_y > screen_height_) {
        pos.y = -pos.h / 2.0f;
        wrapped = true;
    }

    if (wrapped) {
        boid->setPosition(pos.x, pos.y);
    }
}

void BoidManager::limitSpeed(Ball* boid) {
    // Limitar velocidad máxima del boid
    float vx, vy;
    boid->getVelocity(vx, vy);

    float speed = std::sqrt(vx * vx + vy * vy);

    // Limitar velocidad máxima
    if (speed > BOID_MAX_SPEED) {
        vx = (vx / speed) * BOID_MAX_SPEED;
        vy = (vy / speed) * BOID_MAX_SPEED;
        boid->setVelocity(vx, vy);
    }

    // FASE 1.2: Aplicar velocidad mínima (evitar boids estáticos)
    if (speed > 0.0f && speed < BOID_MIN_SPEED) {
        vx = (vx / speed) * BOID_MIN_SPEED;
        vy = (vy / speed) * BOID_MIN_SPEED;
        boid->setVelocity(vx, vy);
    }
}