nightshade 0.13.3

use super::events::{CollisionEvent, CollisionEventKind, ContactForceEvent};
use crate::ecs::world::World;
use rapier3d::na::{Quaternion, UnitQuaternion};
use rapier3d::prelude::*;

pub fn run_physics_systems(world: &mut World) {
    let rigid_body_count = world.resources.physics.rigid_body_set.len();
    let _span = tracing::info_span!("physics", rigid_bodies = rigid_body_count).entered();

    world.resources.physics.clear_events();

    initialize_physics_bodies_system(world);
    sync_collision_listener_system(world);
    process_pending_joints_system(world);

    if world.resources.physics.paused {
        return;
    }

    super::character_controller::character_controller_input_system(world);

    let delta_time = world.resources.window.timing.delta_time;
    let frame_time = delta_time.min(0.25);

    world.resources.physics.accumulator += frame_time;

    let mut substeps_performed = 0;

    while world.resources.physics.accumulator >= world.resources.physics.fixed_timestep
        && substeps_performed < world.resources.physics.max_substeps
    {
        let interpolation_entities: Vec<_> = world
            .core
            .query_entities(crate::ecs::world::PHYSICS_INTERPOLATION)
            .collect();

        for &entity in &interpolation_entities {
            if let Some(interpolation) = world.core.get_physics_interpolation_mut(entity) {
                interpolation.update_previous();
            }
        }

        for &entity in &interpolation_entities {
            let restore = world
                .core
                .get_physics_interpolation(entity)
                .filter(|interp| interp.enabled)
                .map(|interp| (interp.current_translation, interp.current_rotation));
            if let Some((translation, rotation)) = restore
                && let Some(transform) = world.core.get_local_transform_mut(entity)
            {
                transform.translation = translation;
                transform.rotation = rotation;
            }
        }

        sync_transforms_to_physics_system(world);

        super::character_controller::character_controller_system(world);

        super::grab::update_grab_physics(world);

        world.resources.physics.step();

        world.resources.physics.accumulator -= world.resources.physics.fixed_timestep;
        substeps_performed += 1;
    }

    drain_collision_events(world);

    tracing::trace!(substeps = substeps_performed);

    world.resources.physics.interpolation_alpha =
        world.resources.physics.accumulator / world.resources.physics.fixed_timestep;

    sync_transforms_from_physics_system(world);
    physics_interpolation_system(world);
    super::grab::sync_grab_visual(world);

    cleanup_physics_bodies_system(world);

    super::debug::physics_debug_draw_system(world);
}

const TRANSFORM_EPSILON: f32 = 1e-6;

fn transforms_approximately_equal(
    old_translation: &nalgebra_glm::Vec3,
    new_translation: &nalgebra_glm::Vec3,
    old_rotation: &nalgebra_glm::Quat,
    new_rotation: &nalgebra_glm::Quat,
) -> bool {
    let translation_diff = nalgebra_glm::distance(old_translation, new_translation);
    if translation_diff > TRANSFORM_EPSILON {
        return false;
    }
    let rotation_dot = nalgebra_glm::quat_dot(old_rotation, new_rotation).abs();
    rotation_dot > 1.0 - TRANSFORM_EPSILON
}

pub fn sync_transforms_from_physics_system(world: &mut World) {
    let active_count = world.resources.physics.island_manager.active_bodies().len();
    let mut updates = Vec::with_capacity(active_count);

    for handle in world.resources.physics.island_manager.active_bodies() {
        let Some(&entity) = world.resources.physics.handle_to_entity.get(handle) else {
            continue;
        };

        let Some(rb) = world.resources.physics.rigid_body_set.get(*handle) else {
            continue;
        };

        let translation = rb.translation();
        let rotation = rb.rotation();
        let new_translation = nalgebra_glm::vec3(translation.x, translation.y, translation.z);
        let new_rotation = nalgebra_glm::quat(rotation.i, rotation.j, rotation.k, rotation.w);

        let should_update = if let Some(transform) = world.core.get_local_transform(entity) {
            !transforms_approximately_equal(
                &transform.translation,
                &new_translation,
                &transform.rotation,
                &new_rotation,
            )
        } else {
            true
        };

        if should_update {
            updates.push((entity, new_translation, new_rotation));
        }
    }

    for (entity, translation, rotation) in updates {
        if let Some(transform) = world.core.get_local_transform_mut(entity) {
            transform.translation = translation;
            transform.rotation = rotation;
        }

        if let Some(interpolation) = world.core.get_physics_interpolation_mut(entity) {
            interpolation.update_current(translation, rotation);
        }

        world.mark_local_transform_dirty(entity);
    }
}

pub fn sync_transforms_to_physics_system(world: &mut World) {
    let entities: Vec<_> = world
        .core
        .query_entities(crate::ecs::world::LOCAL_TRANSFORM | crate::ecs::world::RIGID_BODY)
        .collect();

    for entity in entities {
        let (transform_data, handle) = {
            let transform = world.core.get_local_transform(entity);
            let rigid_body = world.core.get_rigid_body(entity);

            if let (Some(transform), Some(rigid_body)) = (transform, rigid_body)
                && let Some(handle) = rigid_body.handle
            {
                (
                    Some((transform.translation, transform.rotation)),
                    Some(handle),
                )
            } else {
                (None, None)
            }
        };

        if let (Some((translation, rotation)), Some(handle)) = (transform_data, handle) {
            let rapier_translation = vector![translation.x, translation.y, translation.z];
            let rapier_rotation = UnitQuaternion::new_normalize(Quaternion::new(
                rotation.w, rotation.i, rotation.j, rotation.k,
            ));

            if let Some(rb) = world.resources.physics.get_rigid_body_mut(handle.into()) {
                if rb.body_type() == RigidBodyType::KinematicPositionBased {
                    rb.set_next_kinematic_translation(rapier_translation);
                    rb.set_next_kinematic_rotation(rapier_rotation);
                } else if rb.body_type() == RigidBodyType::Fixed {
                    rb.set_translation(rapier_translation, true);
                    rb.set_rotation(rapier_rotation, true);
                }
            }
        }
    }
}

pub fn initialize_physics_bodies_system(world: &mut World) {
    let entities: Vec<_> = world
        .core
        .query_entities(crate::ecs::world::RIGID_BODY)
        .collect();

    for entity in entities {
        let should_initialize = world
            .core
            .get_rigid_body(entity)
            .map(|rb| rb.handle.is_none())
            .unwrap_or(false);

        if !should_initialize {
            continue;
        }

        let rigid_body_data = world.core.get_rigid_body(entity).cloned();
        let collider_data = world.core.get_collider(entity).cloned();

        if let Some(rigid_body_comp) = rigid_body_data {
            let rigid_body = rigid_body_comp.to_rapier_rigid_body();
            let rapier_handle = world.resources.physics.add_rigid_body(rigid_body);
            let has_listener = world.core.get_collision_listener(entity).is_some();

            if let Some(collider_comp) = collider_data {
                let mut collider = collider_comp.to_rapier_collider();
                if has_listener {
                    collider.set_active_events(
                        ActiveEvents::COLLISION_EVENTS | ActiveEvents::CONTACT_FORCE_EVENTS,
                    );
                }
                world
                    .resources
                    .physics
                    .add_collider(collider, rapier_handle);
            }

            if let Some(rigid_body_mut) = world.core.get_rigid_body_mut(entity) {
                rigid_body_mut.handle = Some(rapier_handle.into());
            }

            world
                .resources
                .physics
                .handle_to_entity
                .insert(rapier_handle, entity);
            world
                .resources
                .physics
                .entity_to_handle
                .insert(entity, rapier_handle);

            let transform_data = world
                .core
                .get_local_transform(entity)
                .map(|t| (t.translation, t.rotation));

            if let Some((translation, rotation)) = transform_data
                && let Some(interpolation) = world.core.get_physics_interpolation_mut(entity)
            {
                interpolation.previous_translation = translation;
                interpolation.previous_rotation = rotation;
                interpolation.current_translation = translation;
                interpolation.current_rotation = rotation;
            }
        }
    }
}

pub fn process_pending_joints_system(world: &mut World) {
    let mut indices_to_remove = Vec::new();
    let mut joints_to_register = Vec::new();

    for (index, pending) in world.resources.physics.pending_joints.iter().enumerate() {
        let parent_rb = world.core.get_rigid_body(pending.parent_entity);
        let child_rb = world.core.get_rigid_body(pending.child_entity);

        match (parent_rb, child_rb) {
            (Some(parent), Some(child)) => {
                if let (Some(parent_handle), Some(child_handle)) = (parent.handle, child.handle) {
                    let joint_handle = world.resources.physics.impulse_joint_set.insert(
                        parent_handle.into(),
                        child_handle.into(),
                        pending.joint,
                        pending.collisions_enabled,
                    );
                    joints_to_register.push((
                        joint_handle,
                        super::resources::JointInfo {
                            parent_entity: pending.parent_entity,
                            child_entity: pending.child_entity,
                            joint_type: pending.joint_type,
                            collisions_enabled: pending.collisions_enabled,
                            spring_rest_length: pending.spring_rest_length,
                            spring_stiffness: pending.spring_stiffness,
                            spring_damping: pending.spring_damping,
                        },
                    ));
                    indices_to_remove.push(index);
                }
            }
            (None, _) | (_, None) => {
                indices_to_remove.push(index);
            }
        }
    }

    for (handle, info) in joints_to_register {
        world.resources.physics.joint_registry.insert(handle, info);
    }

    for index in indices_to_remove.into_iter().rev() {
        world.resources.physics.pending_joints.swap_remove(index);
    }
}

pub fn cleanup_physics_bodies_system(world: &mut World) {
    let mut to_remove = Vec::new();

    for (rapier_handle, _) in world.resources.physics.rigid_body_set.iter() {
        if let Some(&entity) = world.resources.physics.handle_to_entity.get(&rapier_handle) {
            let has_component = world.core.get_rigid_body(entity).is_some();
            if !has_component {
                to_remove.push(rapier_handle);
            }
        }
    }

    for rapier_handle in to_remove {
        world.resources.physics.remove_rigid_body(rapier_handle);
    }
}

pub fn physics_interpolation_system(world: &mut World) {
    let alpha = world.resources.physics.get_interpolation_alpha();

    let interpolation_entities: Vec<_> = world
        .core
        .query_entities(crate::ecs::world::PHYSICS_INTERPOLATION)
        .collect();

    for entity in interpolation_entities {
        let interpolation_data = world.core.get_physics_interpolation(entity).and_then(|i| {
            if i.enabled {
                Some(i.interpolate(alpha))
            } else {
                None
            }
        });

        if let Some((new_translation, new_rotation)) = interpolation_data {
            let should_update = if let Some(transform) = world.core.get_local_transform(entity) {
                !transforms_approximately_equal(
                    &transform.translation,
                    &new_translation,
                    &transform.rotation,
                    &new_rotation,
                )
            } else {
                true
            };

            if should_update {
                if let Some(transform) = world.core.get_local_transform_mut(entity) {
                    transform.translation = new_translation;
                    transform.rotation = new_rotation;
                }
                world.mark_local_transform_dirty(entity);
            }
        }
    }
}

fn sync_collision_listener_system(world: &mut World) {
    let entities: Vec<_> = world
        .core
        .query_entities(crate::ecs::world::COLLISION_LISTENER | crate::ecs::world::COLLIDER)
        .collect();

    for entity in entities {
        let collider_handle = world.core.get_collider(entity).and_then(|c| c.handle);
        if let Some(handle) = collider_handle
            && let Some(collider) = world.resources.physics.collider_set.get_mut(handle.into())
            && !collider
                .active_events()
                .contains(ActiveEvents::COLLISION_EVENTS)
        {
            collider.set_active_events(
                ActiveEvents::COLLISION_EVENTS | ActiveEvents::CONTACT_FORCE_EVENTS,
            );
        }
    }
}

fn drain_collision_events(world: &mut World) {
    let raw_collisions = world
        .resources
        .physics
        .event_collector
        .drain_raw_collisions();
    let raw_forces = world
        .resources
        .physics
        .event_collector
        .drain_raw_contact_forces();

    let mut collision_events = Vec::with_capacity(raw_collisions.len());
    for raw in raw_collisions {
        let entity_a = resolve_collider_to_entity(world, raw.collider_a);
        let entity_b = resolve_collider_to_entity(world, raw.collider_b);
        if let (Some(entity_a), Some(entity_b)) = (entity_a, entity_b) {
            let has_listener = world.core.get_collision_listener(entity_a).is_some()
                || world.core.get_collision_listener(entity_b).is_some();
            if has_listener {
                collision_events.push(CollisionEvent {
                    entity_a,
                    entity_b,
                    kind: if raw.started {
                        CollisionEventKind::Started
                    } else {
                        CollisionEventKind::Stopped
                    },
                    is_sensor: raw.is_sensor,
                });
            }
        }
    }

    let mut contact_force_events = Vec::with_capacity(raw_forces.len());
    for raw in raw_forces {
        let entity_a = resolve_collider_to_entity(world, raw.collider_a);
        let entity_b = resolve_collider_to_entity(world, raw.collider_b);
        if let (Some(entity_a), Some(entity_b)) = (entity_a, entity_b) {
            let has_listener = world.core.get_collision_listener(entity_a).is_some()
                || world.core.get_collision_listener(entity_b).is_some();
            if has_listener {
                contact_force_events.push(ContactForceEvent {
                    entity_a,
                    entity_b,
                    total_force_magnitude: raw.total_force_magnitude,
                });
            }
        }
    }

    world
        .resources
        .physics
        .set_collision_events(collision_events);
    world
        .resources
        .physics
        .set_contact_force_events(contact_force_events);
}

fn resolve_collider_to_entity(
    world: &World,
    collider_handle: ColliderHandle,
) -> Option<freecs::Entity> {
    let collider = world.resources.physics.collider_set.get(collider_handle)?;
    let rb_handle = collider.parent()?;
    world
        .resources
        .physics
        .handle_to_entity
        .get(&rb_handle)
        .copied()
}