use crate::ecs::world::World;
use rapier3d::prelude::*;

pub fn character_controller_input_system(world: &mut World) {
    #[cfg(feature = "openxr")]
    let has_xr_input = world.resources.xr.input.is_some();
    #[cfg(not(feature = "openxr"))]
    let has_xr_input = false;

    if !has_xr_input
        && let Some(gui_state) = &mut world.resources.user_interface.state
        && gui_state.egui_ctx().wants_keyboard_input()
    {
        return;
    }

    let entities: Vec<_> = world
        .query_entities(crate::ecs::world::CHARACTER_CONTROLLER)
        .collect();

    let delta_time = world.resources.physics.fixed_timestep;

    #[cfg(feature = "openxr")]
    let xr_input = world.resources.xr.input.clone();

    for entity in entities {
        let (
            speed,
            acceleration,
            mut velocity,
            can_jump,
            jump_impulse,
            crouch_speed_multiplier,
            sprint_speed_multiplier,
        ) = {
            let Some(character_controller) = world.get_character_controller(entity) else {
                continue;
            };

            (
                character_controller.max_speed,
                character_controller.acceleration,
                character_controller.velocity,
                character_controller.can_jump,
                character_controller.jump_impulse,
                character_controller.crouch_speed_multiplier,
                character_controller.sprint_speed_multiplier,
            )
        };

        let (
            left_key_pressed,
            right_key_pressed,
            forward_key_pressed,
            backward_key_pressed,
            jump_key_pressed,
            crouch_key_pressed,
            sprint_key_pressed,
        ) = {
            let keyboard = &world.resources.input.keyboard;
            (
                keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyA),
                keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyD),
                keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyW),
                keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyS),
                keyboard.is_key_pressed(winit::keyboard::KeyCode::Space),
                keyboard.is_key_pressed(winit::keyboard::KeyCode::ControlLeft)
                    || keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyC),
                keyboard.is_key_pressed(winit::keyboard::KeyCode::ShiftLeft),
            )
        };

        let (gamepad_movement, gamepad_jump, gamepad_crouch, gamepad_sprint) =
            if let Some(gamepad) = crate::ecs::input::queries::query_active_gamepad(world) {
                let left_stick_x = gamepad.value(gilrs::Axis::LeftStickX);
                let left_stick_y = gamepad.value(gilrs::Axis::LeftStickY);
                let jump_button = gamepad.is_pressed(gilrs::Button::South);
                let crouch_button = gamepad.is_pressed(gilrs::Button::RightThumb);
                let sprint_button = gamepad.is_pressed(gilrs::Button::LeftThumb);

                let deadzone = 0.15;
                let left_stick_magnitude =
                    (left_stick_x * left_stick_x + left_stick_y * left_stick_y).sqrt();
                let movement = if left_stick_magnitude > deadzone {
                    let normalized_magnitude = (left_stick_magnitude - deadzone) / (1.0 - deadzone);
                    nalgebra_glm::vec2(left_stick_x, left_stick_y) * normalized_magnitude
                } else {
                    nalgebra_glm::vec2(0.0, 0.0)
                };

                (movement, jump_button, crouch_button, sprint_button)
            } else {
                (nalgebra_glm::vec2(0.0, 0.0), false, false, false)
            };

        #[cfg(feature = "openxr")]
        let (xr_movement, xr_jump, xr_crouch, xr_sprint, xr_combined_yaw) =
            if let Some(ref xr) = xr_input {
                let deadzone = 0.15;
                let stick_magnitude =
                    (xr.thumbstick.x * xr.thumbstick.x + xr.thumbstick.y * xr.thumbstick.y).sqrt();
                let movement = if stick_magnitude > deadzone {
                    let normalized_magnitude = (stick_magnitude - deadzone) / (1.0 - deadzone);
                    nalgebra_glm::vec2(xr.thumbstick.x, xr.thumbstick.y) * normalized_magnitude
                } else {
                    nalgebra_glm::vec2(0.0, 0.0)
                };
                let combined_yaw = xr.head_yaw + xr.player_yaw;
                (
                    movement,
                    xr.a_button,
                    xr.b_button,
                    xr.left_thumbstick_click,
                    Some(combined_yaw),
                )
            } else {
                (nalgebra_glm::vec2(0.0, 0.0), false, false, false, None)
            };

        #[cfg(feature = "openxr")]
        let (forward, right) = if let Some(yaw) = xr_combined_yaw {
            let forward = nalgebra_glm::vec3(-yaw.sin(), 0.0, -yaw.cos());
            let right = nalgebra_glm::vec3(yaw.cos(), 0.0, -yaw.sin());
            (forward, right)
        } else if let Some(camera_entity) = world.resources.active_camera {
            if let Some(camera_transform) = world.get_local_transform(camera_entity) {
                let forward_vec = camera_transform.forward_vector();
                let right_vec = camera_transform.right_vector();
                let forward_horizontal =
                    nalgebra_glm::normalize(&nalgebra_glm::vec3(forward_vec.x, 0.0, forward_vec.z));
                let right_horizontal =
                    nalgebra_glm::normalize(&nalgebra_glm::vec3(right_vec.x, 0.0, right_vec.z));
                (forward_horizontal, right_horizontal)
            } else {
                (
                    nalgebra_glm::vec3(0.0, 0.0, -1.0),
                    nalgebra_glm::vec3(1.0, 0.0, 0.0),
                )
            }
        } else {
            (
                nalgebra_glm::vec3(0.0, 0.0, -1.0),
                nalgebra_glm::vec3(1.0, 0.0, 0.0),
            )
        };

        #[cfg(not(feature = "openxr"))]
        let (forward, right) = if let Some(camera_entity) = world.resources.active_camera {
            if let Some(camera_transform) = world.get_local_transform(camera_entity) {
                let forward_vec = camera_transform.forward_vector();
                let right_vec = camera_transform.right_vector();
                let forward_horizontal =
                    nalgebra_glm::normalize(&nalgebra_glm::vec3(forward_vec.x, 0.0, forward_vec.z));
                let right_horizontal =
                    nalgebra_glm::normalize(&nalgebra_glm::vec3(right_vec.x, 0.0, right_vec.z));
                (forward_horizontal, right_horizontal)
            } else {
                (
                    nalgebra_glm::vec3(0.0, 0.0, -1.0),
                    nalgebra_glm::vec3(1.0, 0.0, 0.0),
                )
            }
        } else {
            (
                nalgebra_glm::vec3(0.0, 0.0, -1.0),
                nalgebra_glm::vec3(1.0, 0.0, 0.0),
            )
        };

        let mut movement_direction = nalgebra_glm::vec3(0.0, 0.0, 0.0);

        if forward_key_pressed {
            movement_direction += forward;
        }
        if backward_key_pressed {
            movement_direction -= forward;
        }
        if left_key_pressed {
            movement_direction -= right;
        }
        if right_key_pressed {
            movement_direction += right;
        }

        if nalgebra_glm::length(&gamepad_movement) > 0.0 {
            movement_direction = forward * gamepad_movement.y + right * gamepad_movement.x;
        }

        #[cfg(feature = "openxr")]
        if nalgebra_glm::length(&xr_movement) > 0.0 {
            movement_direction = forward * xr_movement.y + right * xr_movement.x;
        }

        if nalgebra_glm::length(&movement_direction) > 0.0 {
            movement_direction = nalgebra_glm::normalize(&movement_direction);
        }

        #[cfg(feature = "openxr")]
        let crouch_input_pressed = crouch_key_pressed || gamepad_crouch || xr_crouch;
        #[cfg(not(feature = "openxr"))]
        let crouch_input_pressed = crouch_key_pressed || gamepad_crouch;
        let crouch_input_was_pressed = world
            .get_character_controller(entity)
            .map(|cc| cc.crouch_input_was_pressed)
            .unwrap_or(false);
        let was_crouching = world
            .get_character_controller(entity)
            .map(|cc| cc.is_crouching)
            .unwrap_or(false);

        let is_crouching = if crouch_input_pressed && !crouch_input_was_pressed {
            !was_crouching
        } else {
            was_crouching
        };

        #[cfg(feature = "openxr")]
        let sprint_input_pressed = sprint_key_pressed || gamepad_sprint || xr_sprint;
        #[cfg(not(feature = "openxr"))]
        let sprint_input_pressed = sprint_key_pressed || gamepad_sprint;
        let sprint_input_was_pressed = world
            .get_character_controller(entity)
            .map(|cc| cc.sprint_input_was_pressed)
            .unwrap_or(false);
        let was_sprinting = world
            .get_character_controller(entity)
            .map(|cc| cc.is_sprinting)
            .unwrap_or(false);

        let has_movement_input = nalgebra_glm::length(&movement_direction) > 0.0;
        let is_sprinting = if sprint_input_pressed && !sprint_input_was_pressed {
            !was_sprinting && !is_crouching
        } else if is_crouching || !has_movement_input {
            false
        } else {
            was_sprinting
        };

        let effective_speed = if is_crouching {
            speed * crouch_speed_multiplier
        } else if is_sprinting {
            speed * sprint_speed_multiplier
        } else {
            speed
        };

        let target_velocity = movement_direction * effective_speed;
        let velocity_change = (target_velocity - nalgebra_glm::vec3(velocity.x, 0.0, velocity.z))
            * acceleration
            * delta_time;

        velocity.x += velocity_change.x;
        velocity.z += velocity_change.z;

        let horizontal_speed =
            nalgebra_glm::length(&nalgebra_glm::vec3(velocity.x, 0.0, velocity.z));
        if horizontal_speed > effective_speed {
            let normalized =
                nalgebra_glm::normalize(&nalgebra_glm::vec3(velocity.x, 0.0, velocity.z));
            velocity.x = normalized.x * effective_speed;
            velocity.z = normalized.z * effective_speed;
        }

        let mut can_jump_updated = can_jump;
        #[cfg(feature = "openxr")]
        let jump_pressed = jump_key_pressed || gamepad_jump || xr_jump;
        #[cfg(not(feature = "openxr"))]
        let jump_pressed = jump_key_pressed || gamepad_jump;

        if jump_pressed && can_jump {
            velocity.y = jump_impulse;
            can_jump_updated = false;
        }

        if let Some(controller) = world.get_character_controller_mut(entity) {
            controller.velocity = velocity;
            controller.can_jump = can_jump_updated;
            controller.is_crouching = is_crouching;
            controller.is_sprinting = is_sprinting;
            controller.crouch_input_was_pressed = crouch_input_pressed;
            controller.sprint_input_was_pressed = sprint_input_pressed;
            if is_crouching {
                controller.shape = super::ColliderShape::Capsule {
                    half_height: controller.crouching_half_height,
                    radius: match controller.shape {
                        super::ColliderShape::Capsule { radius, .. } => radius,
                        _ => 0.3,
                    },
                };
            } else {
                controller.shape = super::ColliderShape::Capsule {
                    half_height: controller.standing_half_height,
                    radius: match controller.shape {
                        super::ColliderShape::Capsule { radius, .. } => radius,
                        _ => 0.3,
                    },
                };
            }
        }
    }
}

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

    let delta_time = world.resources.physics.fixed_timestep;
    let gravity_y = world.resources.physics.gravity.y;

    for entity in entities {
        let (mut velocity, shape, controller_config, position) = {
            let character_controller = world.get_character_controller(entity);
            let local_transform = world.get_local_transform(entity);

            if let (Some(cc), Some(lt)) = (character_controller, local_transform) {
                (cc.velocity, cc.shape.clone(), cc.controller, lt.translation)
            } else {
                continue;
            }
        };

        velocity.y += gravity_y * delta_time;

        let desired_translation = vector![
            velocity.x * delta_time,
            velocity.y * delta_time,
            velocity.z * delta_time,
        ];

        let character_shape = match &shape {
            super::ColliderShape::Capsule {
                half_height,
                radius,
            } => SharedShape::capsule_y(*half_height, *radius),
            super::ColliderShape::Ball { radius } => SharedShape::ball(*radius),
            super::ColliderShape::Cuboid { hx, hy, hz } => SharedShape::cuboid(*hx, *hy, *hz),
            _ => SharedShape::capsule_y(0.9, 0.3),
        };

        let character_pos = rapier3d::na::Isometry3::new(
            vector![position.x, position.y, position.z],
            rapier3d::na::Vector3::zeros(),
        );

        let mut collisions = Vec::new();

        let corrected_movement = {
            let physics = &world.resources.physics;
            controller_config.move_shape(
                delta_time,
                &physics.rigid_body_set,
                &physics.collider_set,
                &physics.query_pipeline,
                character_shape.as_ref(),
                &character_pos,
                desired_translation,
                QueryFilter::default(),
                |collision| collisions.push(collision),
            )
        };

        if let Some(local_transform) = world.mutate_local_transform(entity) {
            local_transform.translation.x =
                character_pos.translation.x + corrected_movement.translation.x;
            local_transform.translation.y =
                character_pos.translation.y + corrected_movement.translation.y;
            local_transform.translation.z =
                character_pos.translation.z + corrected_movement.translation.z;
        }

        for collision in &collisions {
            if let Some(collider) = world.resources.physics.collider_set.get(collision.handle)
                && let Some(parent_handle) = collider.parent()
                && let Some(rb) = world
                    .resources
                    .physics
                    .rigid_body_set
                    .get_mut(parent_handle)
                && rb.body_type() == RigidBodyType::Dynamic
            {
                let push_strength = 2.0;
                let impulse = vector![
                    velocity.x * push_strength * delta_time,
                    0.0,
                    velocity.z * push_strength * delta_time
                ];
                rb.apply_impulse(impulse, true);
            }
        }

        if let Some(character_controller) = world.get_character_controller_mut(entity) {
            character_controller.grounded = corrected_movement.grounded;
            character_controller.can_jump = character_controller.grounded;

            if character_controller.grounded && velocity.y < 0.0 {
                velocity.y = 0.0;
            }

            if character_controller.grounded {
                let friction_factor = 1.0 - (8.0 * delta_time).min(1.0);
                velocity.x *= friction_factor;
                velocity.z *= friction_factor;
            }

            character_controller.velocity = velocity;
            character_controller.controller = controller_config;
        }
    }
}

pub fn player_wasd_movement_system(world: &mut World, player_entity: crate::ecs::world::Entity) {
    let Some(camera_entity) = world.resources.active_camera else {
        return;
    };

    let keyboard = &world.resources.input.keyboard;
    let forward = keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyW);
    let backward = keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyS);
    let left = keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyA);
    let right = keyboard.is_key_pressed(winit::keyboard::KeyCode::KeyD);
    let jump_keyboard = keyboard.is_key_pressed(winit::keyboard::KeyCode::Space);

    let gamepad = &world.resources.input.gamepad;
    let (gamepad_move_x, gamepad_move_y, jump_gamepad) = if let Some(gilrs) = &gamepad.gilrs
        && let Some(gamepad_id) = gamepad.gamepad
    {
        if let Some(gamepad) = gilrs.connected_gamepad(gamepad_id) {
            let left_stick_x = gamepad
                .axis_data(gilrs::Axis::LeftStickX)
                .map(|a| a.value())
                .unwrap_or(0.0);
            let left_stick_y = gamepad
                .axis_data(gilrs::Axis::LeftStickY)
                .map(|a| a.value())
                .unwrap_or(0.0);
            let jump = gamepad.is_pressed(gilrs::Button::South);
            (left_stick_x, -left_stick_y, jump)
        } else {
            (0.0, 0.0, false)
        }
    } else {
        (0.0, 0.0, false)
    };

    let jump = jump_keyboard || jump_gamepad;

    let camera_rotation = world
        .get_local_transform(camera_entity)
        .map(|t| t.rotation)
        .unwrap_or_else(nalgebra_glm::quat_identity);

    let forward_dir =
        nalgebra_glm::quat_rotate_vec3(&camera_rotation, &nalgebra_glm::vec3(0.0, 0.0, -1.0));
    let right_dir =
        nalgebra_glm::quat_rotate_vec3(&camera_rotation, &nalgebra_glm::vec3(1.0, 0.0, 0.0));

    let forward_dir = nalgebra_glm::vec3(forward_dir.x, 0.0, forward_dir.z);
    let right_dir = nalgebra_glm::vec3(right_dir.x, 0.0, right_dir.z);

    let forward_dir = if nalgebra_glm::length(&forward_dir) > 0.01 {
        nalgebra_glm::normalize(&forward_dir)
    } else {
        nalgebra_glm::vec3(0.0, 0.0, 0.0)
    };

    let right_dir = if nalgebra_glm::length(&right_dir) > 0.01 {
        nalgebra_glm::normalize(&right_dir)
    } else {
        nalgebra_glm::vec3(0.0, 0.0, 0.0)
    };

    let mut movement = nalgebra_glm::vec3(0.0, 0.0, 0.0);

    if forward {
        movement += forward_dir;
    }
    if backward {
        movement -= forward_dir;
    }
    if left {
        movement -= right_dir;
    }
    if right {
        movement += right_dir;
    }

    if gamepad_move_x.abs() > 0.1 || gamepad_move_y.abs() > 0.1 {
        movement += forward_dir * gamepad_move_y + right_dir * gamepad_move_x;
    }

    if nalgebra_glm::length(&movement) > 0.0 {
        movement = nalgebra_glm::normalize(&movement);
    }

    let dt = world.resources.window.timing.delta_time;

    if let Some(controller) = world.get_character_controller_mut(player_entity) {
        let target_velocity = movement * controller.max_speed;

        controller.velocity.x = nalgebra_glm::lerp_scalar(
            controller.velocity.x,
            target_velocity.x,
            controller.acceleration * dt,
        );
        controller.velocity.z = nalgebra_glm::lerp_scalar(
            controller.velocity.z,
            target_velocity.z,
            controller.acceleration * dt,
        );

        if jump && controller.can_jump {
            controller.velocity.y = controller.jump_impulse;
            controller.can_jump = false;
        }
    }
}
