use crate::ecs::world::World;
use rapier3d::na::{Quaternion, UnitQuaternion};
use rapier3d::prelude::*;

pub fn run_physics_systems(world: &mut World) {
    initialize_physics_bodies_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 entities: Vec<_> = world
            .query_entities(crate::ecs::world::PHYSICS_INTERPOLATION)
            .collect();

        for entity in 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;
    }

    if substeps_performed > 0 {
        world
            .resources
            .physics
            .query_pipeline
            .update(&world.resources.physics.collider_set);
    }

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

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

    let mut updates = Vec::new();

    for entity in &entities {
        if let Some(rigid_body) = world.get_rigid_body(*entity)
            && let Some(handle) = rigid_body.handle
            && let Some(rb) = world.resources.physics.get_rigid_body(handle)
        {
            let translation = rb.translation();
            let rotation = rb.rotation();
            updates.push((
                *entity,
                nalgebra_glm::vec3(translation.x, translation.y, translation.z),
                nalgebra_glm::quat(rotation.w, rotation.i, rotation.j, rotation.k),
            ));
        }
    }

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

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

            if let Some(rigid_body_mut) = world.get_rigid_body_mut(entity) {
                rigid_body_mut.handle = Some(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 cleanup_physics_bodies_system(world: &mut World) {
    let mut to_remove = Vec::new();

    for (handle, _) in world.resources.physics.rigid_body_set.iter() {
        let mut found = false;
        let entities: Vec<_> = world
            .query_entities(crate::ecs::world::RIGID_BODY)
            .collect();

        for entity in entities {
            if let Some(rigid_body_comp) = world.get_rigid_body(entity)
                && rigid_body_comp.handle == Some(handle)
            {
                found = true;
                break;
            }
        }

        if !found {
            to_remove.push(handle);
        }
    }

    for handle in to_remove {
        world.resources.physics.remove_rigid_body(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) {
            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 entities: Vec<_> = world
        .query_entities(
            crate::ecs::world::LOCAL_TRANSFORM | crate::ecs::world::PHYSICS_INTERPOLATION,
        )
        .collect();

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

        if let Some(Some((translation, rotation))) = interpolation_data
            && let Some(transform) = world.get_local_transform_mut(entity)
        {
            transform.translation = translation;
            transform.rotation = rotation;
        }
    }
}
