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();

    initialize_physics_bodies_system(world);
    process_pending_joints_system(world);

    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 active_entities: Vec<_> = world
            .resources
            .physics
            .island_manager
            .active_bodies()
            .iter()
            .filter_map(|handle| {
                world
                    .resources
                    .physics
                    .handle_to_entity
                    .get(handle)
                    .copied()
            })
            .collect();

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

        sync_transforms_to_physics_system(world);
        sync_physics_materials_system(world);

        super::character_controller::character_controller_system(world);

        world.resources.physics.step();

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

    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);

    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.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.get_local_transform_mut(entity) {
            transform.translation = translation;
            transform.rotation = rotation;
        }

        if let Some(interpolation) = world.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
        .query_entities(crate::ecs::world::LOCAL_TRANSFORM | crate::ecs::world::RIGID_BODY)
        .collect();

    for entity in entities {
        let (transform_data, handle) = {
            let transform = world.get_local_transform(entity);
            let rigid_body = world.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
        .query_entities(crate::ecs::world::RIGID_BODY)
        .collect();

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

        if !should_initialize {
            continue;
        }

        let rigid_body_data = world.get_rigid_body(entity).cloned();
        let collider_data = world.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);

            if let Some(collider_comp) = collider_data {
                let collider = collider_comp.to_rapier_collider();
                world
                    .resources
                    .physics
                    .add_collider(collider, rapier_handle);
            }

            if let Some(rigid_body_mut) = world.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
                .get_local_transform(entity)
                .map(|t| (t.translation, t.rotation));

            if let Some((translation, rotation)) = transform_data
                && let Some(interpolation) = world.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.get_rigid_body(pending.parent_entity);
        let child_rb = world.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,
                        },
                    ));
                    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.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 sync_physics_materials_system(world: &mut World) {
    let entities: Vec<_> = world
        .query_entities(crate::ecs::world::COLLIDER | crate::ecs::world::PHYSICS_MATERIAL)
        .collect();

    let mut updates = Vec::new();
    for entity in &entities {
        let collider_handle = world.get_collider(*entity).and_then(|c| c.handle);
        let material_data = world.get_physics_material(*entity).map(|m| {
            (
                m.material.friction,
                m.material.restitution,
                m.material.density,
            )
        });

        if let (Some(handle), Some((friction, restitution, density))) =
            (collider_handle, material_data)
        {
            updates.push((*entity, handle, friction, restitution, density));
        }
    }

    for (entity, handle, friction, restitution, density) in updates {
        if let Some(collider) = world.resources.physics.collider_set.get_mut(handle.into()) {
            collider.set_friction(friction);
            collider.set_restitution(restitution);
        }

        if let Some(collider_mut) = world.get_collider_mut(entity) {
            collider_mut.friction = friction;
            collider_mut.restitution = restitution;
            collider_mut.density = density;
        }
    }
}

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

    let active_bodies: Vec<_> = world
        .resources
        .physics
        .island_manager
        .active_bodies()
        .iter()
        .filter_map(|handle| {
            world
                .resources
                .physics
                .handle_to_entity
                .get(handle)
                .copied()
        })
        .collect();

    for entity in active_bodies {
        let interpolation_data = world.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.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.get_local_transform_mut(entity) {
                    transform.translation = new_translation;
                    transform.rotation = new_rotation;
                }
                world.mark_local_transform_dirty(entity);
            }
        }
    }
}
