// physics_world.cpp - Implementación del mundo físico
// © 2025 Orni Attack

#include "core/physics/physics_world.hpp"

#include <algorithm>
#include <cmath>

#include "core/physics/rigid_body.hpp"

namespace Physics {

void PhysicsWorld::addBody(RigidBody* body) {
    if (body == nullptr) {
        return;
    }
    if (std::ranges::find(bodies_, body) == bodies_.end()) {
        bodies_.push_back(body);
    }
}

void PhysicsWorld::removeBody(RigidBody* body) {
    std::erase(bodies_, body);
}

void PhysicsWorld::update(float dt) {
    integrate(dt);
    if (has_bounds_) {
        resolveBoundsCollisions();
    }
    resolveBodyCollisions();
}

// Integración semi-implícita de Euler:
//   v(t+dt) = v(t) + (F/m) * dt
//   x(t+dt) = x(t) + v(t+dt) * dt
// Más estable que Euler explícito para juegos. Damping exponencial.
void PhysicsWorld::integrate(float dt) {
    for (auto* body : bodies_) {
        if (body == nullptr || body->isStatic()) {
            continue;
        }

        // Aplicar fuerzas acumuladas → aceleración
        const Vec2 acceleration = body->force_accumulator * body->inverse_mass;
        body->velocity += acceleration * dt;

        // Damping exponencial: equivalente a v *= exp(-damping * dt)
        // Aproximación lineal cuando damping*dt es pequeño.
        if (body->linear_damping > 0.0F) {
            const float DAMP = std::exp(-body->linear_damping * dt);
            body->velocity *= DAMP;
        }
        if (body->angular_damping > 0.0F) {
            const float DAMP = std::exp(-body->angular_damping * dt);
            body->angular_velocity *= DAMP;
        }

        // Actualizar posición y rotación
        body->position += body->velocity * dt;
        body->angle += body->angular_velocity * dt;

        body->clearAccumulators();
    }
}

// Rebote contra los 4 bordes del rectángulo bounds_.
// Refleja la componente normal de la velocidad por la restitución.
void PhysicsWorld::resolveBoundsCollisions() {
    const float MIN_X = bounds_.x;
    const float MAX_X = bounds_.x + bounds_.w;
    const float MIN_Y = bounds_.y;
    const float MAX_Y = bounds_.y + bounds_.h;

    for (auto* body : bodies_) {
        if (body == nullptr || body->isStatic()) {
            continue;
        }
        const float R = body->radius;

        // Pared izquierda
        if (body->position.x - R < MIN_X) {
            body->position.x = MIN_X + R;
            if (body->velocity.x < 0.0F) {
                body->velocity.x = -body->velocity.x * body->restitution;
            }
        }
        // Pared derecha
        if (body->position.x + R > MAX_X) {
            body->position.x = MAX_X - R;
            if (body->velocity.x > 0.0F) {
                body->velocity.x = -body->velocity.x * body->restitution;
            }
        }
        // Pared superior
        if (body->position.y - R < MIN_Y) {
            body->position.y = MIN_Y + R;
            if (body->velocity.y < 0.0F) {
                body->velocity.y = -body->velocity.y * body->restitution;
            }
        }
        // Pared inferior
        if (body->position.y + R > MAX_Y) {
            body->position.y = MAX_Y - R;
            if (body->velocity.y > 0.0F) {
                body->velocity.y = -body->velocity.y * body->restitution;
            }
        }
    }
}

// Colisiones cuerpo-cuerpo: O(n²) círculo-círculo + resolución por impulso.
// Para 15 enemigos + 6 balas + 2 naves = ~23 cuerpos → 253 pares. Sobra.
//
// Fórmula del impulso elástico (referencia: Chris Hecker / Box2D):
//   j = -(1 + e) * (v_rel · n) / (1/m_a + 1/m_b)
// donde n es la normal del contacto (de a hacia b) y v_rel = v_a - v_b.
void PhysicsWorld::resolveBodyCollisions() {
    const std::size_t COUNT = bodies_.size();
    for (std::size_t i = 0; i < COUNT; ++i) {
        for (std::size_t j = i + 1; j < COUNT; ++j) {
            auto* a = bodies_[i];
            auto* b = bodies_[j];
            if (a == nullptr || b == nullptr) {
                continue;
            }
            // Dos cuerpos estáticos no necesitan resolución
            if (a->isStatic() && b->isStatic()) {
                continue;
            }

            const Vec2 DELTA = b->position - a->position;
            const float DIST_SQ = DELTA.lengthSquared();
            const float SUM_R = a->radius + b->radius;
            if (DIST_SQ > SUM_R * SUM_R || DIST_SQ <= 0.0F) {
                continue;
            }

            const float DIST = std::sqrt(DIST_SQ);
            const Vec2 NORMAL = DELTA / DIST;  // de A hacia B

            // Corrección posicional (resolver penetración)
            const float PENETRATION = SUM_R - DIST;
            const float TOTAL_INV_MASS = a->inverse_mass + b->inverse_mass;
            if (TOTAL_INV_MASS > 0.0F) {
                const Vec2 CORRECTION = NORMAL * (PENETRATION / TOTAL_INV_MASS);
                if (!a->isStatic()) {
                    a->position -= CORRECTION * a->inverse_mass;
                }
                if (!b->isStatic()) {
                    b->position += CORRECTION * b->inverse_mass;
                }
            }

            // Velocidad relativa proyectada sobre la normal
            const Vec2 V_REL = b->velocity - a->velocity;
            const float VEL_ALONG_NORMAL = V_REL.dot(NORMAL);
            // Si se están separando, no aplicar impulso
            if (VEL_ALONG_NORMAL > 0.0F) {
                continue;
            }

            // Restitución promedio (Box2D usa max; promedio es más permisivo)
            const float E = (a->restitution + b->restitution) * 0.5F;
            const float J = -(1.0F + E) * VEL_ALONG_NORMAL / TOTAL_INV_MASS;
            const Vec2 IMPULSE = NORMAL * J;

            if (!a->isStatic()) {
                a->velocity -= IMPULSE * a->inverse_mass;
            }
            if (!b->isStatic()) {
                b->velocity += IMPULSE * b->inverse_mass;
            }
        }
    }
}

}  // namespace Physics
