nightshade 0.13.3

use crate::ecs::world::World;
use crate::mcp::McpResponse;

pub(crate) fn mcp_add_rigid_body(
    world: &mut World,
    request: crate::mcp::AddRigidBodyRequest,
) -> Result<McpResponse, McpResponse> {
    use crate::ecs::physics::components::RigidBodyComponent;
    use crate::ecs::physics::types::RigidBodyType;

    let entity = super::resolve_entity(world, &request.name)?;

    let rb_type = match request.body_type.to_lowercase().as_str() {
        "dynamic" => RigidBodyType::Dynamic,
        "kinematic" | "kinematic_position" => RigidBodyType::KinematicPositionBased,
        "kinematic_velocity" => RigidBodyType::KinematicVelocityBased,
        "static" | "fixed" => RigidBodyType::Fixed,
        _ => {
            return Err(McpResponse::Error(format!(
                "Unknown body type '{}'. Use: dynamic, kinematic, static",
                request.body_type
            )));
        }
    };

    let mut rb = RigidBodyComponent {
        body_type: rb_type,
        mass: request.mass.unwrap_or(1.0),
        ..Default::default()
    };

    if let Some(lv) = request.linear_velocity {
        rb.linvel = lv;
    }
    if let Some(av) = request.angular_velocity {
        rb.angvel = av;
    }

    if let Some(transform) = world.core.get_local_transform(entity) {
        rb.translation = [
            transform.translation.x,
            transform.translation.y,
            transform.translation.z,
        ];
        rb.rotation = [
            transform.rotation.i,
            transform.rotation.j,
            transform.rotation.k,
            transform.rotation.w,
        ];
    }

    world.core.set_rigid_body(entity, rb);
    Ok(McpResponse::Success(format!(
        "Rigid body added to '{}'",
        request.name
    )))
}

pub(crate) fn mcp_add_collider(
    world: &mut World,
    request: crate::mcp::AddColliderRequest,
) -> Result<McpResponse, McpResponse> {
    use crate::ecs::physics::components::{ColliderComponent, ColliderShape};

    let entity = super::resolve_entity(world, &request.name)?;

    let collider_shape = match request.shape.to_lowercase().as_str() {
        "ball" | "sphere" => {
            let radius = request.size.first().copied().unwrap_or(0.5);
            ColliderShape::Ball { radius }
        }
        "cuboid" | "box" => {
            let hx = request.size.first().copied().unwrap_or(0.5);
            let hy = request.size.get(1).copied().unwrap_or(hx);
            let hz = request.size.get(2).copied().unwrap_or(hx);
            ColliderShape::Cuboid { hx, hy, hz }
        }
        "capsule" => {
            let half_height = request.size.first().copied().unwrap_or(0.5);
            let radius = request.size.get(1).copied().unwrap_or(0.25);
            ColliderShape::Capsule {
                half_height,
                radius,
            }
        }
        "cylinder" => {
            let half_height = request.size.first().copied().unwrap_or(0.5);
            let radius = request.size.get(1).copied().unwrap_or(0.25);
            ColliderShape::Cylinder {
                half_height,
                radius,
            }
        }
        _ => {
            return Err(McpResponse::Error(format!(
                "Unknown collider shape '{}'. Use: ball, cuboid, capsule, cylinder",
                request.shape
            )));
        }
    };

    let collider = ColliderComponent {
        shape: collider_shape,
        friction: request.friction.unwrap_or(0.5),
        restitution: request.restitution.unwrap_or(0.0),
        is_sensor: request.is_sensor.unwrap_or(false),
        ..Default::default()
    };

    world.core.set_collider(entity, collider);
    Ok(McpResponse::Success(format!(
        "Collider added to '{}'",
        request.name
    )))
}

pub(crate) fn mcp_apply_impulse(
    world: &mut World,
    request: crate::mcp::ApplyImpulseRequest,
) -> Result<McpResponse, McpResponse> {
    let torque = request.torque.unwrap_or(false);
    let entity = super::resolve_entity(world, &request.name)?;

    let Some(rb) = world.core.get_rigid_body(entity) else {
        return Err(McpResponse::Error(format!(
            "Entity '{}' has no rigid body",
            request.name
        )));
    };

    let Some(handle) = rb.handle else {
        return Err(McpResponse::Error(format!(
            "Rigid body '{}' not yet initialized in physics world",
            request.name
        )));
    };

    let impulse_vec =
        rapier3d::prelude::vector![request.impulse[0], request.impulse[1], request.impulse[2]];

    if let Some(body) = world
        .resources
        .physics
        .rigid_body_set
        .get_mut(handle.into())
    {
        if torque {
            body.apply_torque_impulse(impulse_vec, true);
        } else {
            body.apply_impulse(impulse_vec, true);
        }
    }

    Ok(McpResponse::Success(format!(
        "Impulse applied to '{}'",
        request.name
    )))
}

pub(crate) fn mcp_apply_force(
    world: &mut World,
    request: crate::mcp::ApplyForceRequest,
) -> Result<McpResponse, McpResponse> {
    let torque = request.torque.unwrap_or(false);
    let entity = super::resolve_entity(world, &request.name)?;

    let Some(rb) = world.core.get_rigid_body(entity) else {
        return Err(McpResponse::Error(format!(
            "Entity '{}' has no rigid body",
            request.name
        )));
    };

    let Some(handle) = rb.handle else {
        return Err(McpResponse::Error(format!(
            "Rigid body '{}' not yet initialized in physics world",
            request.name
        )));
    };

    let force_vec =
        rapier3d::prelude::vector![request.force[0], request.force[1], request.force[2]];

    if let Some(body) = world
        .resources
        .physics
        .rigid_body_set
        .get_mut(handle.into())
    {
        if torque {
            body.add_torque(force_vec, true);
        } else {
            body.add_force(force_vec, true);
        }
    }

    Ok(McpResponse::Success(format!(
        "Force applied to '{}'",
        request.name
    )))
}

pub(crate) fn mcp_set_velocity(
    world: &mut World,
    request: crate::mcp::SetVelocityRequest,
) -> Result<McpResponse, McpResponse> {
    let entity = super::resolve_entity(world, &request.name)?;

    let Some(rb) = world.core.get_rigid_body(entity) else {
        return Err(McpResponse::Error(format!(
            "Entity '{}' has no rigid body",
            request.name
        )));
    };

    let Some(handle) = rb.handle else {
        return Err(McpResponse::Error(format!(
            "Rigid body '{}' not yet initialized in physics world",
            request.name
        )));
    };

    if let Some(body) = world
        .resources
        .physics
        .rigid_body_set
        .get_mut(handle.into())
    {
        if let Some(lv) = request.linear {
            body.set_linvel(rapier3d::prelude::vector![lv[0], lv[1], lv[2]], true);
        }
        if let Some(av) = request.angular {
            body.set_angvel(rapier3d::prelude::vector![av[0], av[1], av[2]], true);
        }
    }

    Ok(McpResponse::Success(format!(
        "Velocity set on '{}'",
        request.name
    )))
}