// physics_world.cpp - Implementación del mundo físico
// © 2026 JailDesigner

#include "core/physics/physics_world.hpp"

#include <cmath>

#include "core/physics/rigid_body.hpp"

namespace Physics {

    void PhysicsWorld::addBody(RigidBody* body) {
        if (body == nullptr) {
            return;
        }
        for (const auto* b : bodies_) {
            if (b == body) {
                return;
            }
        }
        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.
    namespace {

        // Resol col·lisió contra un parell paret-axis (mín i màx).
        // pos/vel són les referències al component de l'axis actiu (x o y);
        // contact_perp és la coordenada del component perpendicular (la fixa de la
        // paret a l'eix actiu — usada per al contact_point).
        void resolveAxis(float& pos,
            float& vel,
            float radius,
            float min_val,
            float max_val,
            float restitution,
            bool axis_is_x,
            float contact_perp,
            const PhysicsWorld::BoundsHitCallback& callback) {
            // Cara mínima (esquerra o superior)
            if (pos - radius < min_val) {
                pos = min_val + radius;
                if (vel < 0.0F) {
                    if (callback) {
                        const Vec2 CONTACT = axis_is_x
                            ? Vec2{.x = min_val, .y = contact_perp}
                            : Vec2{.x = contact_perp, .y = min_val};
                        callback(BoundsHit{.contact_point = CONTACT, .impact_speed = -vel});
                    }
                    vel = -vel * restitution;
                }
            }
            // Cara màxima (dreta o inferior)
            if (pos + radius > max_val) {
                pos = max_val - radius;
                if (vel > 0.0F) {
                    if (callback) {
                        const Vec2 CONTACT = axis_is_x
                            ? Vec2{.x = max_val, .y = contact_perp}
                            : Vec2{.x = contact_perp, .y = max_val};
                        callback(BoundsHit{.contact_point = CONTACT, .impact_speed = vel});
                    }
                    vel = -vel * restitution;
                }
            }
        }

    }  // namespace

    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;
            }
            // Eix X (esquerra/dreta): contact_perp = y actual del cos.
            resolveAxis(body->position.x, body->velocity.x, body->radius, MIN_X, MAX_X, body->restitution, /*axis_is_x=*/true, body->position.y, bounds_hit_callback_);
            // Eix Y (superior/inferior): contact_perp = x actual (ja clampejada en l'eix X).
            resolveAxis(body->position.y, body->velocity.y, body->radius, MIN_Y, MAX_Y, body->restitution, /*axis_is_x=*/false, body->position.x, bounds_hit_callback_);
        }
    }

    // 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) {
                    resolveBodyPair(*a, *b);
                }
            }
        }
    }

    void PhysicsWorld::resolveBodyPair(RigidBody& a, RigidBody& b) {
        // Dos cuerpos estáticos no necesitan resolución
        if (a.isStatic() && b.isStatic()) {
            return;
        }

        // Un cuerpo con radius=0 es cinemático puro (ej. la bala) y no participa
        // en body-body. La detecció de gameplay (Physics::checkCollision) usa
        // el radius de l'entity (no el del body) i s'encarrega d'aquesta parella.
        if (a.radius <= 0.0F || b.radius <= 0.0F) {
            return;
        }

        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) {
            return;
        }

        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) {
            return;
        }

        // 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
