vibe3_physics/source/boids_mgr/boid_manager.cpp

#include "boid_manager.hpp"

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

#include "ball.hpp"                 // for Ball
#include "engine.hpp"               // for Engine (si se necesita)
#include "scene/scene_manager.hpp"  // for SceneManager
#include "state/state_manager.hpp"  // for StateManager
#include "ui/ui_manager.hpp"        // 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()
      ,
      separation_radius_(BOID_SEPARATION_RADIUS),
      alignment_radius_(BOID_ALIGNMENT_RADIUS),
      cohesion_radius_(BOID_COHESION_RADIUS),
      separation_weight_(BOID_SEPARATION_WEIGHT),
      alignment_weight_(BOID_ALIGNMENT_WEIGHT),
      cohesion_weight_(BOID_COHESION_WEIGHT),
      max_speed_(BOID_MAX_SPEED),
      min_speed_(BOID_MIN_SPEED),
      max_force_(BOID_MAX_FORCE),
      boundary_margin_(BOID_BOUNDARY_MARGIN),
      boundary_weight_(BOID_BOUNDARY_WEIGHT) {
}

BoidManager::~BoidManager() = default;

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;
        float vy;
        ball->getVelocity(vx, vy);
        if (vx == 0.0f && vy == 0.0f) {
            // Velocidad aleatoria entre -60 y +60 px/s (time-based)
            vx = ((rand() % 200 - 100) / 100.0f) * 60.0f;
            vy = ((rand() % 200 - 100) / 100.0f) * 60.0f;
            ball->setVelocity(vx, vy);
        }
    }

    // Mostrar notificación (solo si NO estamos en modo demo o logo)
    if ((state_mgr_ != nullptr) && (ui_mgr_ != nullptr) && 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_ != nullptr) && (ui_mgr_ != nullptr) && 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 (time-based)
    for (auto& ball : balls) {
        float vx;
        float vy;
        ball->getVelocity(vx, vy);

        SDL_FRect pos = ball->getPosition();
        pos.x += vx * delta_time;  // time-based
        pos.y += vy * delta_time;

        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, 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 < 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 = (separation_radius_ - distance) / 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;
        float vy;
        boid->getVelocity(vx, vy);
        vx += steer_x * separation_weight_ * delta_time;
        vy += steer_y * 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, 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 < alignment_radius_) {
            float other_vx;
            float 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;
        float vy;
        boid->getVelocity(vx, vy);
        float steer_x = (avg_vx - vx) * alignment_weight_ * delta_time;
        float steer_y = (avg_vy - vy) * 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 > max_force_) {
            steer_x = (steer_x / steer_mag) * max_force_;
            steer_y = (steer_y / steer_mag) * 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, 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 < 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) * cohesion_weight_ * delta_time;
            float steer_y = (dy_to_center / distance_to_center) * 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 > max_force_) {
                steer_x = (steer_x / steer_mag) * max_force_;
                steer_y = (steer_y / steer_mag) * max_force_;
            }

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

void BoidManager::applyBoundaries(Ball* boid) const {
    // NUEVA IMPLEMENTACIÓN: Bordes como obstáculos (repulsión en lugar de wrapping)
    // Cuando un boid se acerca a un borde, se aplica una fuerza alejándolo
    SDL_FRect pos = boid->getPosition();
    float center_x = pos.x + (pos.w / 2.0f);
    float center_y = pos.y + (pos.h / 2.0f);

    float steer_x = 0.0f;
    float steer_y = 0.0f;

    // Borde izquierdo (x < boundary_margin_)
    if (center_x < boundary_margin_) {
        float distance = center_x;  // Distancia al borde (0 = colisión)
        if (distance < boundary_margin_) {
            // Fuerza proporcional a cercanía: 0% en margen, 100% en colisión
            float repulsion_strength = (boundary_margin_ - distance) / boundary_margin_;
            steer_x += repulsion_strength;  // Empujar hacia la derecha
        }
    }

    // Borde derecho (x > screen_width_ - boundary_margin_)
    if (center_x > screen_width_ - boundary_margin_) {
        float distance = screen_width_ - center_x;
        if (distance < boundary_margin_) {
            float repulsion_strength = (boundary_margin_ - distance) / boundary_margin_;
            steer_x -= repulsion_strength;  // Empujar hacia la izquierda
        }
    }

    // Borde superior (y < boundary_margin_)
    if (center_y < boundary_margin_) {
        float distance = center_y;
        if (distance < boundary_margin_) {
            float repulsion_strength = (boundary_margin_ - distance) / boundary_margin_;
            steer_y += repulsion_strength;  // Empujar hacia abajo
        }
    }

    // Borde inferior (y > screen_height_ - boundary_margin_)
    if (center_y > screen_height_ - boundary_margin_) {
        float distance = screen_height_ - center_y;
        if (distance < boundary_margin_) {
            float repulsion_strength = (boundary_margin_ - distance) / boundary_margin_;
            steer_y -= repulsion_strength;  // Empujar hacia arriba
        }
    }

    // Aplicar fuerza de repulsión si hay alguna
    if (steer_x != 0.0f || steer_y != 0.0f) {
        float vx;
        float vy;
        boid->getVelocity(vx, vy);

        // Normalizar fuerza de repulsión (para que todas las direcciones tengan la misma intensidad)
        float steer_mag = std::sqrt((steer_x * steer_x) + (steer_y * steer_y));
        if (steer_mag > 0.0f) {
            steer_x /= steer_mag;
            steer_y /= steer_mag;
        }

        // Aplicar aceleración de repulsión (time-based)
        // boundary_weight_ es más fuerte que separation para garantizar que no escapen
        vx += steer_x * boundary_weight_ * (1.0f / 60.0f);  // Simular delta_time fijo para independencia
        vy += steer_y * boundary_weight_ * (1.0f / 60.0f);
        boid->setVelocity(vx, vy);
    }
}

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

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

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

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