gizmo-physics-rigid 0.1.7

use gizmo_physics_core::{Collider, Transform};
use crate::components::{RigidBody, Velocity};use crate::world::PhysicsWorld;
use gizmo_core::entity::Entity;
use gizmo_core::query::{Mut, Query};
use gizmo_core::world::World;

/// Exclusive system that updates the entire physics simulation.
/// It reads all rigid and soft bodies from the ECS, steps the physics world,
/// and writes the transformed positions and velocities back to the ECS.
#[tracing::instrument(skip_all, name = "physics_step_system")]
pub fn physics_step_system(world: &World, dt: f32) {
    // Record profiler scope (if FrameProfiler resource is available)
    if let Ok(mut profiler) = world.try_get_resource_mut::<gizmo_core::profiler::FrameProfiler>() {
        profiler.begin_scope("physics_total");
    }

    // 1. Acquire PhysicsWorld Resource
    let mut physics_world = match world.try_get_resource_mut::<PhysicsWorld>() {
        Ok(res) => res,
        Err(e) => {
            tracing::info!("[Physics] FAILED TO GET PhysicsWorld Resource: {:?}", e);
            return;
        }
    };

    // 2. Gather Compound Shapes (Read Locks Only)
    let mut compound_shapes_map = std::collections::HashMap::new();
    {
        if let Some(query) = world.query::<(
            &Collider,
            &Transform,
            &RigidBody,
            gizmo_core::query::Without<gizmo_core::pool::Pooled>,
            gizmo_core::query::Without<gizmo_core::component::IsDeleted>,
        )>() {
            let mut children_query = world.query::<&gizmo_core::component::Children>();
            let trans_query = world.query::<&Transform>();
            let col_query = world.query::<&Collider>();

            for (id, (col, transform, _rb, _, _)) in query.iter() {
                let mut compound_shapes = Vec::new();
                compound_shapes.push((
                    gizmo_physics_core::Transform::default(),
                    Box::new(col.shape.clone()),
                ));

                // Check children recursively
                let mut stack = vec![id];
                while let Some(curr_id) = stack.pop() {
                    if let Some(children_query_ref) = &mut children_query {
                        if let Some(children) = children_query_ref.get(curr_id) {
                            for &child_id in &children.0 {
                                stack.push(child_id);
                                if let (Some(tq), Some(cq)) = (&trans_query, &col_query) {
                                    if let (Some(child_trans), Some(child_col)) = (tq.get(child_id), cq.get(child_id)) {
                                        let inv_rot = transform.rotation.inverse();
                                        let local_pos =
                                            inv_rot.mul_vec3(child_trans.position - transform.position);
                                        let local_rot = inv_rot * child_trans.rotation;

                                        let local_t = gizmo_physics_core::Transform::new(local_pos)
                                            .with_rotation(local_rot);
                                        compound_shapes
                                            .push((local_t, Box::new(child_col.shape.clone())));
                                    }
                                }
                            }
                        }
                    }
                }

                // Create a single Collider for this RigidBody
                let final_collider = if compound_shapes.is_empty() {
                    Collider::default() // Should technically not be simulated
                } else if compound_shapes.len() == 1 {
                    // Single collider, avoid nesting in Compound
                    let (_t, s) = compound_shapes.remove(0);
                    Collider {
                        shape: *s,
                        ..Default::default()
                    }
                } else {
                    Collider {
                        shape: gizmo_physics_core::ColliderShape::Compound(compound_shapes),
                        ..Default::default()
                    }
                };

                compound_shapes_map.insert(id, final_collider);
            }
        }
    }

    // 3. Query Rigid Bodies (Write Locks)
    let mut rigid_bodies = Vec::new();
    if let Some(mut query) = world.query::<(
        Mut<RigidBody>,
        Mut<Transform>,
        Mut<Velocity>,
        gizmo_core::query::Without<gizmo_core::pool::Pooled>,
        gizmo_core::query::Without<gizmo_core::component::IsDeleted>,
    )>() {
        for (id, (rb, transform, vel, _, _)) in query.iter_mut() {
            if let Some(final_collider) = compound_shapes_map.remove(&id) {
                rigid_bodies.push((Entity::new(id, 0), *rb, *transform, *vel, final_collider));
            }
        }
    } else {
        tracing::info!("[Physics] FAILED TO BORROW RigidBody/Transform/Velocity Mutably!");
    }

    // 4. Step Simulation
    physics_world.sync_bodies(rigid_bodies.iter());

    physics_world.step(dt).expect("Gizmo Physics Engine encountered a critical numerical error (NaN, Infinity, or Overflow) and halted!");

    // Sync back to rigid_bodies so vehicles/ECS writeback works
    for i in 0..physics_world.entities.len() {
        let entity_id = physics_world.entities[i].id();
        if let Some((_, rb, trans, vel, _)) =
            rigid_bodies.iter_mut().find(|(e, ..)| e.id() == entity_id)
        {
            *rb = physics_world.rigid_bodies[i];
            *trans = physics_world.transforms[i];
            *vel = physics_world.velocities[i];
        }
    }

    // 5. Write back to ECS (Rigid Bodies)
    if !rigid_bodies.is_empty() {
        if let Some(query) = world.query::<(
            Mut<RigidBody>,
            Mut<Transform>,
            Mut<Velocity>,
            gizmo_core::query::Without<gizmo_core::pool::Pooled>,
        )>() {
            for (entity, rb, transform, vel, _collider) in rigid_bodies {
                if let Some((mut ecs_rb, mut ecs_trans, mut ecs_vel, _)) = query.get(entity.id()) {
                    *ecs_rb = rb;
                    *ecs_trans = transform;
                    *ecs_vel = vel;
                }
            }
        }
    }

    

    // 7. Dispatch Events
    if let Ok(mut trigger_queue) =
        world.try_get_resource_mut::<gizmo_core::event::Events<gizmo_physics_core::TriggerEvent>>()
    {
        for event in &physics_world.trigger_events {
            trigger_queue.send(event.clone());
        }
    }

    if let Ok(mut collision_queue) =
        world.try_get_resource_mut::<gizmo_core::event::Events<gizmo_physics_core::CollisionEvent>>()
    {
        for event in &physics_world.collision_events {
            collision_queue.send(event.clone());
        }
    }

    if physics_world.step_once {
        physics_world.step_once = false;
    }

    // Close profiler scope
    drop(physics_world); // PhysicsWorld lock'unu bırak
    if let Ok(mut profiler) = world.try_get_resource_mut::<gizmo_core::profiler::FrameProfiler>() {
        profiler.end_scope("physics_total");
    }
}

/// System that processes collision events and breaks objects that exceed their threshold.
pub fn physics_fracture_system(world: &World, dt: f32) {
    use crate::components::Breakable;
    use gizmo_core::commands::Commands;
    use gizmo_core::system::SystemParam;

    let physics_world = match world.try_get_resource::<PhysicsWorld>() {
        Ok(res) => res,
        Err(_) => return,
    };

    let mut commands = match Commands::fetch(world, dt) {
        Ok(c) => c,
        Err(_) => return,
    };

    let mut shattered = std::collections::HashSet::new();

    let query_opt = Query::<(
        gizmo_core::query::Mut<Breakable>,
        &Transform,
        &Collider,
        &Velocity,
        gizmo_core::query::Without<gizmo_core::pool::Pooled>,
    )>::new(world);
    let query = match query_opt {
        Some(q) => q,
        None => return,
    };

    for event in &physics_world.collision_events {
        let mut max_impulse = 0.0;
        let mut impact_normal = gizmo_math::Vec3::ZERO;
        let mut impact_point = gizmo_math::Vec3::ZERO;

        for contact in &event.contact_points {
            if contact.normal_impulse > max_impulse {
                max_impulse = contact.normal_impulse;
                impact_normal = contact.normal;
                impact_point = contact.point;
            }
        }

        // Fallback: estimate impact from relative velocity when solver impulse is unavailable
        if max_impulse <= 0.0 && !event.contact_points.is_empty() {
            // Look up velocities of both entities to estimate impact force
            let vel_a = physics_world
                .entity_index_map
                .get(&event.entity_a.id())
                .map(|&idx| physics_world.velocities[idx].linear)
                .unwrap_or(gizmo_math::Vec3::ZERO);
            let vel_b = physics_world
                .entity_index_map
                .get(&event.entity_b.id())
                .map(|&idx| physics_world.velocities[idx].linear)
                .unwrap_or(gizmo_math::Vec3::ZERO);
            let mass_a = physics_world
                .entity_index_map
                .get(&event.entity_a.id())
                .map(|&idx| physics_world.rigid_bodies[idx].mass)
                .unwrap_or(1.0);
            let mass_b = physics_world
                .entity_index_map
                .get(&event.entity_b.id())
                .map(|&idx| physics_world.rigid_bodies[idx].mass)
                .unwrap_or(1.0);

            let rel_speed = (vel_b - vel_a).length();
            let reduced_mass = if mass_a > 0.0 && mass_b > 0.0 {
                (mass_a * mass_b) / (mass_a + mass_b)
            } else {
                mass_a.max(mass_b)
            };
            max_impulse = rel_speed * reduced_mass;
            if let Some(contact) = event.contact_points.first() {
                impact_normal = contact.normal;
                impact_point = contact.point;
            }
        }

        if max_impulse <= 0.0 {
            continue;
        }

        // Check Entity A
        if !shattered.contains(&event.entity_a.id()) {
            if let Some((mut breakable, transform, collider, vel, _)) =
                query.get(event.entity_a.id())
            {
                if !breakable.is_broken && max_impulse > breakable.threshold {
                    breakable.current_health -= max_impulse;
                    if breakable.current_health <= 0.0 {
                        breakable.is_broken = true;
                        shattered.insert(event.entity_a.id());
                        shatter_entity(
                            &mut commands,
                            event.entity_a,
                            &breakable,
                            transform,
                            collider,
                            vel,
                            -impact_normal,
                            impact_point,
                        );
                    }
                }
            }
        }

        // Check Entity B
        if !shattered.contains(&event.entity_b.id()) {
            if let Some((mut breakable, transform, collider, vel, _)) =
                query.get(event.entity_b.id())
            {
                if !breakable.is_broken && max_impulse > breakable.threshold {
                    breakable.current_health -= max_impulse;
                    if breakable.current_health <= 0.0 {
                        breakable.is_broken = true;
                        shattered.insert(event.entity_b.id());
                        shatter_entity(
                            &mut commands,
                            event.entity_b,
                            &breakable,
                            transform,
                            collider,
                            vel,
                            impact_normal,
                            impact_point,
                        );
                    }
                }
            }
        }
    }
    drop(query);
}

fn shatter_entity(
    commands: &mut gizmo_core::commands::Commands,
    entity: Entity,
    breakable: &crate::components::Breakable,
    transform: &Transform,
    collider: &Collider,
    vel: &Velocity,
    impact_direction: gizmo_math::Vec3,
    _impact_point: gizmo_math::Vec3,
) {
    use crate::fracture::voronoi_shatter;

    // We only support shattering boxes for now
    let extents = match &collider.shape {
        gizmo_physics_core::ColliderShape::Box(b) => b.half_extents,
        _ => return, // Cannot shatter non-boxes easily with our voronoi yet
    };

    // Despawn the original entity
    commands.entity(entity).despawn();

    // Generate chunks
    let chunks = voronoi_shatter(extents, breakable.max_pieces, 42);

    for chunk in chunks {
        // Create new convex hull colliders or approximated boxes for the chunks.
        // For simplicity, we approximate each chunk with a small sphere or box based on its volume.
        // A full implementation would use ConvexHull shapes.
        let radius = (chunk.volume * 0.1).powf(1.0 / 3.0).max(0.1);

        // Offset chunk center by parent's transform
        let world_offset = transform.rotation * chunk.center_of_mass;
        let mut new_transform = *transform;
        new_transform.position += world_offset;

        // Give chunks a slight explosive velocity outwards from the center of mass
        let mut new_vel = *vel;
        let outward = chunk.center_of_mass.normalize_or_zero();
        new_vel.linear += outward * 2.0 + impact_direction * 5.0; // Explosion effect

        let chunk_collider = Collider::sphere(radius).with_material(collider.material);
        let mut rb = RigidBody::new(chunk.volume * collider.material.density, 0.0, 0.0, true);
        rb.update_inertia_from_collider(&chunk_collider);

        commands
            .spawn()
            .insert(rb)
            .insert(chunk_collider)
            .insert(new_transform)
            .insert(new_vel);
    }
}

/// System that checks for Explosion components and applies outward forces
/// to all rigid bodies and soft body nodes within the radius.
pub fn physics_explosion_system(world: &World, dt: f32) {
    use crate::components::{Explosion, ExplosionFalloff};
    use gizmo_core::commands::Commands;
    use gizmo_core::system::SystemParam;

    let mut commands = match Commands::fetch(world, dt) {
        Ok(c) => c,
        Err(_) => return,
    };

    let explosion_query_opt = Query::<(
        &Explosion,
        &Transform,
        gizmo_core::query::Without<gizmo_core::pool::Pooled>,
    )>::new(world);
    let mut active_explosions = Vec::new();

    if let Some(exp_query) = &explosion_query_opt {
        for (ent_id, (explosion, transform, _)) in exp_query.iter() {
            if explosion.is_active {
                // Apply offset to transform position
                active_explosions.push((
                    Entity::new(ent_id, 0),
                    *explosion,
                    transform.position + explosion.offset,
                ));
            }
        }
    }

    if active_explosions.is_empty() {
        return; // Nothing to explode
    }

    let mut shattered = std::collections::HashSet::new();

    // Helper closure to calculate falloff intensity
    let calculate_intensity = |dist: f32, radius: f32, falloff: ExplosionFalloff| -> f32 {
        if dist >= radius {
            return 0.0;
        }
        match falloff {
            ExplosionFalloff::None => 1.0,
            ExplosionFalloff::Linear => 1.0 - (dist / radius),
            ExplosionFalloff::Quadratic => {
                let ratio = 1.0 - (dist / radius);
                ratio * ratio
            }
        }
    };

    // Check for Breakables that should shatter
    let breakable_query_opt = Query::<(
        gizmo_core::query::Mut<crate::components::Breakable>,
        &Transform,
        &Collider,
        &Velocity,
        gizmo_core::query::Without<gizmo_core::pool::Pooled>,
    )>::new(world);
    if let Some(breakable_query) = &breakable_query_opt {
        for (_exp_entity, explosion, exp_pos) in &active_explosions {
            for (id, (mut breakable, transform, collider, vel, _)) in breakable_query.iter() {
                if breakable.is_broken || shattered.contains(&id) {
                    continue;
                }

                let diff = transform.position - *exp_pos;
                let dist_sq = diff.length_squared();

                if dist_sq < explosion.force_radius * explosion.force_radius && dist_sq > 0.001 {
                    let dist = dist_sq.sqrt();
                    let intensity =
                        calculate_intensity(dist, explosion.force_radius, explosion.falloff);
                    let impulse_mag = explosion.force * intensity;

                    if impulse_mag > breakable.threshold {
                        breakable.current_health -= explosion.damage * intensity;
                        if breakable.current_health <= 0.0 {
                            breakable.is_broken = true;
                            shattered.insert(id);
                            let dir = diff / dist;
                            let mut exp_vel = *vel;
                            exp_vel.linear += dir * impulse_mag * 0.1; // Estimate mass
                            shatter_entity(
                                &mut commands,
                                Entity::new(id, 0),
                                &breakable,
                                transform,
                                collider,
                                &exp_vel,
                                dir,
                                transform.position,
                            );
                        }
                    }
                }
            }
        }
    }

    // Apply to Rigid Bodies
    let rb_query_opt = Query::<(
        Mut<RigidBody>,
        &Transform,
        Mut<Velocity>,
        gizmo_core::query::Without<gizmo_core::pool::Pooled>,
    )>::new(world);
    if let Some(rb_query) = &rb_query_opt {
        for (_exp_entity, explosion, exp_pos) in &active_explosions {
            for (id, (rb, transform, mut vel, _)) in rb_query.iter() {
                if !rb.is_dynamic() || shattered.contains(&id) {
                    continue;
                }

                let diff = transform.position - *exp_pos;
                let dist_sq = diff.length_squared();

                if dist_sq < explosion.force_radius * explosion.force_radius && dist_sq > 0.001 {
                    let dist = dist_sq.sqrt();
                    let dir = diff / dist;

                    let intensity =
                        calculate_intensity(dist, explosion.force_radius, explosion.falloff);
                    let impulse_mag = explosion.force * intensity;

                    // Apply instantaneous velocity change
                    vel.linear += dir * impulse_mag * rb.inv_mass();
                }
            }
        }
    }

    // Despawn the explosions so they don't trigger again
    // Note: If game logic needs to read explosion damage, it must run BEFORE the physics_explosion_system in the schedule!
    for (exp_entity, _, _) in active_explosions {
        commands.entity(exp_entity).despawn();
    }
}