nightshade 0.13.3

use crate::ecs::world::World;

#[derive(Clone, Debug)]
pub struct GrabConfig {
    pub stiffness: f32,
    pub damping_ratio: f32,
    pub max_force: f32,
    pub angular_damping: f32,
}

impl Default for GrabConfig {
    fn default() -> Self {
        Self {
            stiffness: 150.0,
            damping_ratio: 1.0,
            max_force: 80.0,
            angular_damping: 0.95,
        }
    }
}

#[derive(Clone, Debug, Default)]
pub struct GrabState {
    pub entity: Option<crate::ecs::world::Entity>,
    pub camera_entity: Option<crate::ecs::world::Entity>,
    pub local_grab_offset: nalgebra_glm::Vec3,
    pub distance: f32,
    pub min_distance: f32,
    pub max_distance: f32,
    pub config: GrabConfig,
}

impl GrabState {
    pub fn grab(
        &mut self,
        entity: crate::ecs::world::Entity,
        distance: f32,
        min_distance: f32,
        max_distance: f32,
        local_grab_offset: nalgebra_glm::Vec3,
    ) {
        self.entity = Some(entity);
        self.distance = distance.clamp(min_distance, max_distance);
        self.min_distance = min_distance;
        self.max_distance = max_distance;
        self.local_grab_offset = local_grab_offset;
    }

    pub fn release(&mut self) {
        self.entity = None;
        self.camera_entity = None;
    }

    pub fn adjust_distance(&mut self, delta: f32) {
        self.distance = (self.distance + delta).clamp(self.min_distance, self.max_distance);
    }

    pub fn is_holding(&self) -> bool {
        self.entity.is_some()
    }
}

pub(crate) fn update_grab_physics(world: &mut World) {
    let grab = &world.resources.physics.grab;
    let Some(entity) = grab.entity else {
        return;
    };

    let Some(rigid_body_component) = world.core.get_rigid_body(entity) else {
        return;
    };
    let Some(handle) = rigid_body_component.handle else {
        return;
    };

    let Some(camera_entity) = world.resources.physics.grab.camera_entity else {
        return;
    };
    let Some(camera_transform) = world.core.get_global_transform(camera_entity) else {
        return;
    };
    let target_position = camera_transform.translation()
        + camera_transform.forward_vector() * world.resources.physics.grab.distance;

    let local_grab_offset = world.resources.physics.grab.local_grab_offset;
    let config = world.resources.physics.grab.config.clone();
    let dt = world.resources.physics.fixed_timestep.max(0.001);

    let Some(rigid_body) = world
        .resources
        .physics
        .rigid_body_set
        .get_mut(handle.into())
    else {
        return;
    };

    let body_pos = *rigid_body.translation();
    let body_rot = *rigid_body.rotation();
    let body_position = nalgebra_glm::vec3(body_pos.x, body_pos.y, body_pos.z);
    let body_quat = nalgebra_glm::quat(body_rot.w, body_rot.i, body_rot.j, body_rot.k);

    let rotated_offset = nalgebra_glm::quat_rotate_vec3(&body_quat, &local_grab_offset);
    let grab_world_position = body_position + rotated_offset;

    let error = target_position - grab_world_position;

    let mass = rigid_body.mass().max(0.1);

    let critical_damping = 2.0 * (config.stiffness * mass).sqrt();
    let damping = critical_damping * config.damping_ratio;

    let spring_force = error * config.stiffness;
    let current_vel = *rigid_body.linvel();
    let current_velocity = nalgebra_glm::vec3(current_vel.x, current_vel.y, current_vel.z);
    let damping_force = -current_velocity * damping;
    let mut total_force = spring_force + damping_force;

    let force_magnitude = nalgebra_glm::length(&total_force);
    let max_force_for_mass = config.max_force * mass;
    if force_magnitude > max_force_for_mass {
        total_force *= max_force_for_mass / force_magnitude;
    }

    let acceleration = total_force / mass;
    let new_velocity = current_velocity + acceleration * dt;

    rigid_body.set_linvel(
        rapier3d::math::Vector::new(new_velocity.x, new_velocity.y, new_velocity.z),
        true,
    );

    let current_angvel = *rigid_body.angvel();
    let angular_decay = (-config.angular_damping * dt * 60.0).exp();
    rigid_body.set_angvel(current_angvel * angular_decay, true);
}

pub(crate) fn sync_grab_visual(world: &mut World) {
    let Some(grabbed_entity) = world.resources.physics.grab.entity else {
        return;
    };
    let Some(camera_entity) = world.resources.physics.grab.camera_entity else {
        return;
    };

    let distance = world.resources.physics.grab.distance;
    let local_grab_offset = world.resources.physics.grab.local_grab_offset;

    let camera_local = world
        .core
        .get_local_transform(camera_entity)
        .map(|t| (t.translation, t.rotation));
    let Some((camera_offset, camera_rotation)) = camera_local else {
        return;
    };

    let parent_entity = world.core.get_parent(camera_entity).and_then(|p| p.0);

    let player_position = parent_entity
        .and_then(|player| world.core.get_local_transform(player))
        .map(|t| t.translation)
        .unwrap_or(nalgebra_glm::Vec3::zeros());

    let camera_world_position = player_position + camera_offset;
    let camera_forward =
        nalgebra_glm::quat_rotate_vec3(&camera_rotation, &nalgebra_glm::vec3(0.0, 0.0, -1.0));

    let target_position = camera_world_position + camera_forward * distance;

    let Some(rb_component) = world.core.get_rigid_body(grabbed_entity) else {
        return;
    };
    let Some(handle) = rb_component.handle else {
        return;
    };
    let Some(rigid_body) = world.resources.physics.rigid_body_set.get(handle.into()) else {
        return;
    };

    let body_pos = rigid_body.translation();
    let body_rot = rigid_body.rotation();
    let physics_position = nalgebra_glm::vec3(body_pos.x, body_pos.y, body_pos.z);
    let body_rotation = nalgebra_glm::quat(body_rot.w, body_rot.i, body_rot.j, body_rot.k);

    let rotated_offset = nalgebra_glm::quat_rotate_vec3(&body_rotation, &local_grab_offset);
    let ideal_visual = target_position - rotated_offset;

    let max_divergence = 0.2;
    let divergence = nalgebra_glm::distance(&ideal_visual, &physics_position);
    let visual_position = if divergence <= max_divergence {
        ideal_visual
    } else {
        nalgebra_glm::lerp(
            &physics_position,
            &ideal_visual,
            max_divergence / divergence,
        )
    };

    if let Some(transform) = world.core.get_local_transform_mut(grabbed_entity) {
        transform.translation = visual_position;
        transform.rotation = body_rotation;
    }
    world.mark_local_transform_dirty(grabbed_entity);
}

pub fn release_grab_physics(world: &mut World) {
    let Some(entity) = world.resources.physics.grab.entity else {
        return;
    };

    if let Some(rigid_body_component) = world.core.get_rigid_body(entity)
        && let Some(handle) = rigid_body_component.handle
        && let Some(rigid_body) = world
            .resources
            .physics
            .rigid_body_set
            .get_mut(handle.into())
    {
        rigid_body.reset_forces(true);
        rigid_body.reset_torques(true);

        let pos = rigid_body.translation();
        let rot = rigid_body.rotation();
        let translation = nalgebra_glm::vec3(pos.x, pos.y, pos.z);
        let rotation = nalgebra_glm::quat(rot.w, rot.i, rot.j, rot.k);

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

    world.resources.physics.grab.release();
}

pub fn throw_grab_physics(world: &mut World, direction: nalgebra_glm::Vec3, strength: f32) {
    let Some(entity) = world.resources.physics.grab.entity else {
        return;
    };

    let Some(rigid_body_component) = world.core.get_rigid_body(entity) else {
        world.resources.physics.grab.release();
        return;
    };
    let Some(handle) = rigid_body_component.handle else {
        world.resources.physics.grab.release();
        return;
    };
    let Some(rigid_body) = world
        .resources
        .physics
        .rigid_body_set
        .get_mut(handle.into())
    else {
        world.resources.physics.grab.release();
        return;
    };

    rigid_body.reset_forces(true);
    rigid_body.reset_torques(true);

    let throw_velocity = direction * strength;
    rigid_body.set_linvel(
        rapier3d::math::Vector::new(throw_velocity.x, throw_velocity.y, throw_velocity.z),
        true,
    );

    let pos = rigid_body.translation();
    let rot = rigid_body.rotation();
    let translation = nalgebra_glm::vec3(pos.x, pos.y, pos.z);
    let rotation = nalgebra_glm::quat(rot.w, rot.i, rot.j, rot.k);

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

    world.resources.physics.grab.release();
}