class_name StickmanLocomotion
extends Node
## Owns navigation, walking/gait, terrain following, animated falls, and landings.
## StickmanRig schedules this component and exposes its public facade.

enum TerrainMotion { DISABLED, AIRBORNE, GROUNDED, LANDING_IMPACT }

const TERRAIN_PROBE_UP: float = 96.0
const TERRAIN_PROBE_DOWN: float = 700.0
const TERRAIN_NORMAL_MIN_Y: float = -0.15
const TERRAIN_SUPPORT_RANGE: float = 160.0
const TERRAIN_MAX_FOOT_SURFACE_DROP: float = 72.0
const TERRAIN_FOOT_GUARD_HEIGHT: float = 24.0
const TERRAIN_CENTER_PROBE_OFFSETS: Array[float] = [-24.0, 0.0, 24.0]
const TERRAIN_FOOT_PROBE_OFFSETS: Array[float] = [-4.0, 0.0, 4.0]
const TERRAIN_GRAVITY: float = 1400.0
const TERRAIN_MAX_FALL_SPEED: float = 1800.0
const RECOVERY_PROBE_START_OFFSET: float = 50.0
const RECOVERY_PROBE_END_OFFSET: float = 1500.0
const RECOVERY_PROBE_TOLERANCE: float = 200.0
const RECOVERY_PROBE_X_OFFSETS: Array[float] = [0.0, 25.0, -25.0, 50.0, -50.0, 100.0, -100.0]
const AIR_DRAG_X: float = 0.75
const AIR_POSE_START_SPEED: float = 240.0
const AIR_POSE_FULL_SPEED: float = 1200.0
const AIR_POSE_RESPONSE: float = 10.0
const AIR_HAND_FORWARD: float = 220.0
const AIR_HAND_FORWARD_START_SPEED: float = 80.0
const AIR_HAND_FORWARD_FULL_SPEED: float = 520.0
const AIR_HAND_LIFT: float = 680.0
const AIR_HAND_FLAIL_AMPLITUDE: Vector2 = Vector2(20.0, 16.0)
const AIR_FOOT_FLAIL_AMPLITUDE: Vector2 = Vector2(14.0, 10.0)
const AIR_FLAIL_HZ: float = 12.0
const HARD_LANDING_MIN_FALL_HEIGHT: float = 80.0
const HARD_LANDING_MAX_FALL_HEIGHT: float = 900.0
const LANDING_COMPRESSION_MIN: float = 32.0
const LANDING_COMPRESSION_MAX: float = 160.0
const LANDING_HAND_DROP_MIN: float = 270.0
const LANDING_HAND_DROP_MAX: float = 520.0
const LANDING_COMPRESS_DURATION: float = 0.08
const LANDING_RECOVER_DURATION: float = 0.22
const PELVIS_MAX_OFFSET: float = 240.0
const PELVIS_RESPONSE: float = 10.0
const FOOT_OFFSET: Vector2 = Vector2(0.0, -385.0)
const NAV_AGENT_LOCAL_POS: Vector2 = Vector2(0.0, 385.0)
const ARRIVE_DISTANCE: float = 8.0
const NAV_PATH_DESIRED_DISTANCE: float = 8.0
const NAV_TARGET_DESIRED_DISTANCE: float = 12.0
const LATCH_PROBE_MAX_FRAMES: int = 6
const WALK_CYCLE_DURATION: float = 0.8

const HEAD_COLLISION_RADIUS: float = 90.0
const TORSO_COLLISION_RADIUS: float = 20.0
const ARM_COLLISION_RADIUS: float = 12.0
const LEG_COLLISION_RADIUS: float = 14.0
const HEAD_COLLISION_FALLBACK_CENTER: Vector2 = Vector2(0.0, -72.0)
const COLLISION_LAYER: int = 1
const COLLISION_PARTS: Dictionary = {
	"head": {"visual": "Head", "bone": "Torso/Head", "shape": "circle", "radius": HEAD_COLLISION_RADIUS},
	"torso": {"visual": "Body", "bone": "Torso", "shape": "capsule", "radius": TORSO_COLLISION_RADIUS},
	"left_upper_arm": {"visual": "LeftUpperArm", "bone": "Torso/LeftUpperArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
	"left_lower_arm": {"visual": "LeftLowerArm", "bone": "Torso/LeftUpperArm/LeftLowerArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
	"right_upper_arm": {"visual": "RightUpperArm", "bone": "Torso/RightUpperArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
	"right_lower_arm": {"visual": "RightLowerArm", "bone": "Torso/RightUpperArm/RightLowerArm", "shape": "capsule", "radius": ARM_COLLISION_RADIUS},
	"left_upper_leg": {"visual": "LeftUpperLeg", "bone": "Torso/LeftUpperLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
	"left_lower_leg": {"visual": "LeftLowerLeg", "bone": "Torso/LeftUpperLeg/LeftLowerLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
	"right_upper_leg": {"visual": "RightUpperLeg", "bone": "Torso/RightUpperLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
	"right_lower_leg": {"visual": "RightLowerLeg", "bone": "Torso/RightUpperLeg/RightLowerLeg", "shape": "capsule", "radius": LEG_COLLISION_RADIUS},
}
const REACTION_BASE_ANGLE: float = 0.35
const REACTION_REFERENCE_SPEED: float = 400.0
const REACTION_MAX_ANGLE: float = 0.6
const REACTION_LOCAL_X_EPSILON: float = 0.05
const REACTION_TARGETS: Dictionary = {
	"head": [{"joint": "Head", "weight": 1.0, "decay": 0.50}],
	"torso": [{"joint": "Torso", "weight": 0.5, "decay": 0.40}],
	"left_upper_arm": [{"joint": "LeftUpperArm", "weight": 1.0, "decay": 0.40}],
	"left_lower_arm": [{"joint": "LeftLowerArm", "weight": 1.0, "decay": 0.35}, {"joint": "LeftUpperArm", "weight": 0.4, "decay": 0.25}],
	"right_upper_arm": [{"joint": "RightUpperArm", "weight": 1.0, "decay": 0.40}],
	"right_lower_arm": [{"joint": "RightLowerArm", "weight": 1.0, "decay": 0.35}, {"joint": "RightUpperArm", "weight": 0.4, "decay": 0.25}],
	"left_upper_leg": [{"joint": "LeftUpperLeg", "weight": 1.0, "decay": 0.40}],
	"left_lower_leg": [{"joint": "LeftLowerLeg", "weight": 1.0, "decay": 0.35}, {"joint": "LeftUpperLeg", "weight": 0.4, "decay": 0.25}],
	"right_upper_leg": [{"joint": "RightUpperLeg", "weight": 1.0, "decay": 0.40}],
	"right_lower_leg": [{"joint": "RightLowerLeg", "weight": 1.0, "decay": 0.35}, {"joint": "RightUpperLeg", "weight": 0.4, "decay": 0.25}],
}

signal arrived(target: Vector2)

var _rig: StickmanRig = null
var foot_contacts: Dictionary = {"Left_Leg": {"valid": false, "locked": false}, "Right_Leg": {"valid": false, "locked": false}}
var terrain_pelvis_offset: float = 0.0
var terrain_air_velocity: Vector2 = Vector2.ZERO
var terrain_motion: TerrainMotion = TerrainMotion.DISABLED
var terrain_following_enabled: bool = false
var _air_time: float = 0.0
var air_pose_weight: float = 0.0
var air_pose_base: Dictionary = {}
var _air_entry_root_y: float = 0.0
var air_fall_distance: float = 0.0
var _landing_elapsed: float = 0.0
var landing_compression: float = 0.0
var _landing_hand_drop: float = 0.0
var _landing_torso_base: Vector2 = Vector2.ZERO
var _landing_hand_start: Dictionary = {}
var _landing_contacts: Dictionary = {}
var _terrain_paused_walk_animation: bool = false
var _nav_agent: NavigationAgent2D = null
var _nav_anchor: Node2D = null
var _walking: bool = false
var _walk_target_feet: Vector2 = Vector2.ZERO
var _walk_speed_current: float = 300.0
var _walk_mode: String = "nav"
var _walk_mode_latched: bool = false
var _walk_latch_probe_frames: int = 0
var _walk_settle_frames: int = 0
var _walk_done: bool = false
var _leg_base_markers: Dictionary = {}
var _last_grounded_walk_velocity_x: float = 0.0
var collision_bodies: Dictionary = {}
var collision_lengths: Dictionary = {}
var _collision_visuals: Dictionary = {}
var reaction_bones: Dictionary = {}
var impact_reactions: Dictionary = {}

func configure(rig: StickmanRig) -> void:
	_rig = rig

	for leg_name: String in ["Left_Leg", "Right_Leg"]:
		var base: Marker2D = _rig.get_node_or_null(NodePath("AnimationTargets/%s_Base" % leg_name)) as Marker2D
		if base == null:
			push_warning("StickmanLocomotion: missing AnimationTargets/%s_Base." % leg_name)
		else:
			_leg_base_markers[leg_name] = base
	_nav_anchor = Node2D.new()
	_nav_anchor.name = "NavigationAgentAnchor"
	_nav_anchor.position = NAV_AGENT_LOCAL_POS
	_rig.add_child(_nav_anchor)
	_nav_agent = NavigationAgent2D.new()
	_nav_agent.name = "NavigationAgent2D"
	_nav_agent.path_desired_distance = NAV_PATH_DESIRED_DISTANCE
	_nav_agent.target_desired_distance = NAV_TARGET_DESIRED_DISTANCE
	_nav_agent.path_max_distance = 100.0
	_nav_agent.max_speed = _rig.walk_speed
	_nav_agent.avoidance_enabled = false
	_nav_anchor.add_child(_nav_agent)


func setup_collision_bodies() -> void:
	collision_bodies.clear()
	collision_lengths.clear()
	_collision_visuals.clear()
	reaction_bones.clear()
	if _rig == null or _rig._body_container == null or _rig._skeleton == null:
		return
	for key: String in COLLISION_PARTS:
		var definition: Dictionary = COLLISION_PARTS[key]
		var bone: Bone2D = _rig._skeleton.get_node_or_null(NodePath(str(definition["bone"]))) as Bone2D
		if bone == null or not is_instance_valid(bone):
			push_warning("StickmanLocomotion: missing reaction bone '%s'." % str(definition["bone"]))
			continue
		var joint_name: String = "Torso" if str(definition["visual"]) == "Body" else str(definition["visual"])
		reaction_bones[joint_name] = bone
		var visual: Node2D = _rig._body_container.get_node_or_null(NodePath(str(definition["visual"]))) as Node2D
		if visual == null:
			push_warning("StickmanLocomotion: missing collision visual for '%s'." % key)
			continue
		var proxy := AnimatableBody2D.new()
		proxy.name = "CollisionBody"
		proxy.collision_layer = COLLISION_LAYER
		proxy.collision_mask = COLLISION_LAYER
		proxy.sync_to_physics = false
		proxy.set_meta("stickman_rig", _rig)
		proxy.set_meta("part_name", key)
		var shape_node := CollisionShape2D.new()
		shape_node.name = "CollisionShape2D"
		if str(definition["shape"]) == "circle":
			var circle := CircleShape2D.new()
			var bounds: Dictionary = mounted_visual_bounds_local(visual)
			if bool(bounds["valid"]):
				var rect: Rect2 = bounds["rect"] as Rect2
				shape_node.position = rect.get_center()
				circle.radius = maxf(float(definition["radius"]), maxf(rect.size.x, rect.size.y) * 0.5)
			else:
				shape_node.position = HEAD_COLLISION_FALLBACK_CENTER
				circle.radius = float(definition["radius"])
			shape_node.shape = circle
		else:
			var length: float = absf(_rig._head_bone.position.y) if key == "torso" and _rig._head_bone != null else bone.length
			collision_lengths[key] = length
			var capsule := CapsuleShape2D.new()
			capsule.height = length
			capsule.radius = float(definition["radius"])
			shape_node.shape = capsule
			shape_node.position = Vector2(0.0, length * 0.5)
		proxy.add_child(shape_node)
		visual.add_child(proxy)
		proxy.top_level = true
		proxy.global_transform = visual.global_transform
		collision_bodies[key] = proxy
		_collision_visuals[key] = visual


func sync_collision_bodies() -> void:
	for key: String in collision_bodies:
		var proxy: AnimatableBody2D = collision_bodies.get(key) as AnimatableBody2D
		var visual: Node2D = _collision_visuals.get(key) as Node2D
		if proxy != null and visual != null and is_instance_valid(proxy) and is_instance_valid(visual):
			proxy.global_transform = visual.global_transform


func mounted_visual_bounds_local(part: Node2D) -> Dictionary:
	var points: Array[Vector2] = []
	var nodes: Array[Node] = [part]
	while not nodes.is_empty():
		var child: Node = nodes.pop_back()
		if child is Line2D:
			for point: Vector2 in (child as Line2D).points:
				points.append(part.to_local((child as Node2D).to_global(point)))
		elif child is Polygon2D:
			for point: Vector2 in (child as Polygon2D).polygon:
				points.append(part.to_local((child as Node2D).to_global(point)))
		for grandchild: Node in child.get_children():
			nodes.append(grandchild)
	if points.is_empty():
		return {"valid": false, "rect": Rect2()}
	var rect := Rect2(points[0], Vector2.ZERO)
	for point: Vector2 in points:
		rect = rect.expand(point)
	return {"valid": true, "rect": rect}


func set_collision_bodies_enabled(enabled: bool) -> void:
	var layer: int = COLLISION_LAYER if enabled else 0
	for proxy: AnimatableBody2D in collision_bodies.values():
		if proxy != null and is_instance_valid(proxy):
			proxy.collision_layer = layer
			proxy.collision_mask = layer


func apply_collision_reaction(part_name: String, impact_direction: Vector2, impact_speed: float, impact_position: Vector2 = Vector2.INF) -> void:
	if _rig == null or _rig.state != StickmanRig.RigState.ANIMATED or impact_direction.is_zero_approx() or impact_speed <= 0.0:
		return
	for target: Dictionary in REACTION_TARGETS.get(part_name, []):
		var joint: String = str(target["joint"])
		var bone: Bone2D = reaction_bones.get(joint) as Bone2D
		if bone == null or not is_instance_valid(bone):
			continue
		var direction: float = collision_contact_direction(part_name, impact_position, impact_direction)
		if is_zero_approx(direction):
			var local_dir: Vector2 = bone.global_transform.affine_inverse().basis_xform(impact_direction).normalized()
			direction = signf(local_dir.x) if absf(local_dir.x) > REACTION_LOCAL_X_EPSILON else 0.0
		if is_zero_approx(direction): direction = -1.0 if joint.begins_with("Left") else 1.0
		var angle: float = direction * minf(REACTION_MAX_ANGLE, REACTION_BASE_ANGLE * impact_speed / REACTION_REFERENCE_SPEED) * float(target["weight"])
		var old: Dictionary = impact_reactions.get(joint, {})
		if old.is_empty():
			impact_reactions[joint] = {"angle": angle, "strength": 1.0, "decay": float(target["decay"]), "base_rotation": bone.rotation}
		else:
			if absf(angle) >= absf(float(old.get("angle", 0.0))):
				old["angle"] = angle
				old["decay"] = float(target["decay"])
			old["strength"] = 1.0
			impact_reactions[joint] = old


func collision_contact_direction(part_name: String, impact_position: Vector2, impact_direction: Vector2) -> float:
	if not impact_position.is_finite(): return 0.0
	var proxy: AnimatableBody2D = collision_bodies.get(part_name) as AnimatableBody2D
	if proxy == null or not is_instance_valid(proxy): return 0.0
	var shape_node: CollisionShape2D = proxy.get_node_or_null(NodePath("CollisionShape2D")) as CollisionShape2D
	if shape_node == null: return 0.0
	var side: float = proxy.to_local(impact_position).x - shape_node.position.x
	if absf(side) <= REACTION_LOCAL_X_EPSILON: return 0.0
	var local_force: Vector2 = proxy.global_transform.affine_inverse().basis_xform(impact_direction).normalized()
	return signf(side * local_force.y) if absf(local_force.y) > REACTION_LOCAL_X_EPSILON else signf(side)


func clear_impact_reactions(restore_bones: bool = false) -> void:
	if restore_bones:
		for joint: String in impact_reactions:
			var reaction: Dictionary = impact_reactions[joint]
			var bone: Bone2D = reaction_bones.get(joint) as Bone2D
			if bone != null and is_instance_valid(bone) and reaction.has("base_rotation"):
				bone.rotation = float(reaction["base_rotation"])
	impact_reactions.clear()


func apply_impact_reactions(delta: float) -> void:
	if _rig == null or _rig.state != StickmanRig.RigState.ANIMATED:
		clear_impact_reactions()
		return
	for joint: String in impact_reactions.keys().duplicate():
		var reaction: Dictionary = impact_reactions[joint]
		var bone: Bone2D = reaction_bones.get(joint) as Bone2D
		if bone == null or not is_instance_valid(bone):
			impact_reactions.erase(joint)
			continue
		var base_rotation: float = float(reaction.get("base_rotation", bone.rotation))
		bone.rotation = base_rotation + float(reaction["angle"]) * float(reaction["strength"])
		reaction["strength"] = float(reaction["strength"]) - delta / maxf(float(reaction["decay"]), 0.001)
		if float(reaction["strength"]) <= 0.0:
			bone.rotation = base_rotation
			impact_reactions.erase(joint)
		else:
			impact_reactions[joint] = reaction

func tick_movement(delta: float, _rig_state: int) -> void:
	if _rig == null:
		return
	terrain_pelvis_offset = 0.0
	if terrain_following_enabled and terrain_motion == TerrainMotion.GROUNDED:
		_last_grounded_walk_velocity_x = 0.0
	_update_walking(delta)
	_update_terrain_following(delta)
	_settle_walk_markers()

func tick_post_skeleton(delta: float, _rig_state: int) -> void:
	if _rig != null:
		apply_impact_reactions(delta)
		sync_collision_bodies()

func walk_to(target: Vector2, speed: float = -1.0) -> bool:
	if _rig == null or _rig.state != StickmanRig.RigState.ANIMATED:
		push_warning("StickmanRig: walk_to ignored; not ANIMATED.")
		return false
	if _rig._is_action_queue_blocked():
		push_warning("StickmanRig: walk_to ignored during landing impact.")
		return false
	if _nav_agent == null or _nav_anchor == null:
		push_warning("StickmanLocomotion: walk_to ignored; navigation is unavailable.")
		return false
	_walk_target_feet = target
	_walk_speed_current = speed if speed > 0.0 else _rig.walk_speed
	_nav_agent.max_speed = _walk_speed_current
	_nav_agent.target_position = target
	var dx: float = target.x - _rig.global_position.x
	if dx < -0.5:
		_rig.set_facing_profile(StickmanRig.FacingProfile.LEFT)
	elif dx > 0.5:
		_rig.set_facing_profile(StickmanRig.FacingProfile.RIGHT)
	_rig._set_limb_mods_enabled(true)
	if _rig._anim_player != null and is_instance_valid(_rig._anim_player) and _rig._anim_player.has_animation("walk_right"):
		_rig._anim_player.play("walk_right")
	_walking = true
	_walk_done = false
	_walk_mode = "nav"
	_walk_mode_latched = false
	_walk_latch_probe_frames = 0
	_walk_settle_frames = 0
	return true

func cancel_walk(restore_idle_pose: bool = true) -> void:
	if _rig == null:
		return
	var current_limb_rotations: Dictionary = _rig._capture_limb_rotations()
	if _rig._anim_player != null and is_instance_valid(_rig._anim_player):
		_rig._anim_player.stop(true)
	if _nav_agent != null and _nav_anchor != null:
		_nav_agent.target_position = _nav_anchor.global_position
	_walking = false
	_walk_done = false
	_walk_mode_latched = false
	_walk_latch_probe_frames = 0
	_walk_settle_frames = 0
	if restore_idle_pose:
		_rig._apply_idle_limb_solver_mode(current_limb_rotations)

func is_walking() -> bool:
	return _walking


func ensure_grounded_walk_animation() -> void:
	if _rig == null or _rig.state != StickmanRig.RigState.ANIMATED or not _walking:
		return
	if terrain_motion != TerrainMotion.GROUNDED or blocks_walking():
		return
	if _rig._anim_player == null or not is_instance_valid(_rig._anim_player) or not _rig._anim_player.has_animation("walk_right"):
		return
	if _terrain_paused_walk_animation:
		if _rig._anim_player.is_playing():
			_terrain_paused_walk_animation = false
		else:
			_resume_walk_animation()
	elif not _rig._anim_player.is_playing():
		_rig._anim_player.play("walk_right")

func get_foot_offset() -> Vector2:
	return FOOT_OFFSET

func consume_walk_completed() -> bool:
	if not _walk_done:
		return false
	_walk_done = false
	return true

func _update_walking(delta: float) -> void:
	if not _walking:
		return
	if _rig.state != StickmanRig.RigState.ANIMATED:
		cancel_walk(false)
		return
	if blocks_walking() or _nav_agent == null:
		return
	if NavigationServer2D.map_get_iteration_id(_nav_agent.get_navigation_map()) == 0:
		return
	if not _walk_mode_latched:
		if _walk_latch_probe_frames < LATCH_PROBE_MAX_FRAMES:
			_walk_latch_probe_frames += 1
			_nav_agent.get_next_path_position()
			if _nav_agent.is_target_reachable():
				_walk_mode_latched = true
				_walk_mode = "nav"
			else:
				return
		else:
			_walk_mode_latched = true
			_walk_mode = "direct"
	var next_feet: Vector2 = _nav_agent.get_next_path_position()
	var final_root: Vector2 = _walk_target_feet + FOOT_OFFSET
	var dist_to_final: float = absf(_rig.global_position.x - final_root.x)
	var root_target: Vector2
	if _walk_mode == "nav":
		if _nav_agent.is_navigation_finished() and dist_to_final <= 2.0 * ARRIVE_DISTANCE:
			_rig.global_position.x = final_root.x
			_finish_walk()
			return
		# A freshly synchronized agent may yield its own feet as the first
		# waypoint. Treat that zero-progress waypoint as the final segment.
		var next_progress: float = next_feet.x - _rig.global_position.x
		var remaining_progress: float = final_root.x - _rig.global_position.x
		if dist_to_final <= 2.0 * ARRIVE_DISTANCE or remaining_progress * next_progress <= 0.0:
			root_target = Vector2(final_root.x, _rig.global_position.y)
		else:
			root_target = Vector2(next_feet.x, _rig.global_position.y)
	else:
		root_target = Vector2(final_root.x, _rig.global_position.y)
	var before_x: float = _rig.global_position.x
	_rig.global_position = _rig.global_position.move_toward(root_target, _walk_speed_current * delta)
	_last_grounded_walk_velocity_x = (_rig.global_position.x - before_x) / delta if delta > 0.0 else 0.0
	if dist_to_final <= ARRIVE_DISTANCE or absf(_rig.global_position.x - final_root.x) <= ARRIVE_DISTANCE:
		_rig.global_position.x = final_root.x
		_finish_walk()

func _settle_walk_markers() -> void:
	if _walk_settle_frames > 0:
		_walk_settle_frames -= 1
		_rig._restore_standing_markers()

func _finish_walk() -> void:
	var final_limb_rotations: Dictionary = _rig._capture_limb_rotations()
	if _rig._anim_player != null and is_instance_valid(_rig._anim_player):
		_rig._anim_player.stop(true)
	_walking = false
	_rig._apply_idle_limb_solver_mode(final_limb_rotations)
	_walk_settle_frames = 0
	_walk_done = true
	arrived.emit(_walk_target_feet)

func begin_terrain_fall() -> void:
	if _rig == null:
		return
	terrain_following_enabled = true
	terrain_pelvis_offset = 0.0
	_terrain_paused_walk_animation = false
	_enter_airborne(0.0)
	_reset_foot_contacts()

func set_terrain_following_enabled(enabled: bool) -> void:
	if enabled:
		if terrain_motion == TerrainMotion.DISABLED:
			begin_terrain_fall()
		return
	terrain_following_enabled = false
	_cancel_landing_impact(true)
	_restore_air_pose()
	terrain_pelvis_offset = 0.0
	terrain_air_velocity = Vector2.ZERO
	terrain_motion = TerrainMotion.DISABLED
	_terrain_paused_walk_animation = false
	_reset_foot_contacts()
	for leg_name: String in ["Left_Leg", "Right_Leg"]:
		var base: Marker2D = _leg_base_markers.get(leg_name) as Marker2D
		var target: Marker2D = _rig._get_ik_marker(leg_name)
		if base != null and target != null:
			target.position = base.position

func prepare_for_ragdoll() -> void:
	if terrain_motion != TerrainMotion.DISABLED:
		set_terrain_following_enabled(false)

func reset_for_ragdoll_handoff() -> void:
	_cancel_landing_impact(false)
	air_pose_base.clear()
	air_pose_weight = 0.0
	_air_time = 0.0
	_air_entry_root_y = 0.0
	air_fall_distance = 0.0
	terrain_motion = TerrainMotion.DISABLED
	terrain_air_velocity = Vector2.ZERO
	_reset_foot_contacts()

func is_action_blocked() -> bool:
	return terrain_motion == TerrainMotion.LANDING_IMPACT

func blocks_walking() -> bool:
	return terrain_following_enabled and terrain_motion != TerrainMotion.GROUNDED

func _reset_foot_contacts() -> void:
	for leg_name: String in foot_contacts:
		foot_contacts[leg_name] = {"valid": false, "locked": false}

func _probe_terrain_below(world_point: Vector2, probe_down: float = TERRAIN_PROBE_DOWN) -> Dictionary:
	var space_state: PhysicsDirectSpaceState2D = _rig.get_world_2d().direct_space_state
	if space_state == null:
		return {"valid": false}
	var query := PhysicsRayQueryParameters2D.create(world_point + Vector2.UP * TERRAIN_PROBE_UP, world_point + Vector2.DOWN * probe_down)
	query.collision_mask = 1
	query.collide_with_areas = false
	query.exclude = _rig.get_collision_body_rids()
	var hit: Dictionary = space_state.intersect_ray(query)
	if hit.is_empty() or not (hit.get("collider") is TerrainBlock):
		return {"valid": false}
	var normal: Vector2 = hit.get("normal", Vector2.ZERO)
	if normal.y > TERRAIN_NORMAL_MIN_Y:
		return {"valid": false}
	return {"valid": true, "world_pos": hit.get("position", world_point), "normal": normal}

func _probe_terrain_fan(world_point: Vector2, offsets: Array[float]) -> Dictionary:
	var best: Dictionary = {"valid": false}
	for x_offset: float in offsets:
		var hit: Dictionary = _probe_terrain_below(world_point + Vector2(x_offset, 0.0))
		if bool(hit.get("valid", false)) and (not bool(best.get("valid", false)) or (hit["world_pos"] as Vector2).y < (best["world_pos"] as Vector2).y):
			best = hit
	return best

func probe_ground_y(world_x: float, top_y: float, bottom_y: float, exclude: Array[RID]) -> float:
	var best_y := bottom_y
	var best_distance := INF
	if _rig == null:
		return best_y
	var space_state: PhysicsDirectSpaceState2D = _rig.get_world_2d().direct_space_state
	if space_state == null:
		return best_y
	for offset: float in RECOVERY_PROBE_X_OFFSETS:
		var query := PhysicsRayQueryParameters2D.create(
			Vector2(world_x + offset, top_y - RECOVERY_PROBE_START_OFFSET),
			Vector2(world_x + offset, bottom_y + RECOVERY_PROBE_END_OFFSET))
		query.collision_mask = 1
		query.collide_with_areas = false
		query.collide_with_bodies = true
		query.exclude = exclude
		var hit := space_state.intersect_ray(query)
		if hit.is_empty():
			continue
		var hit_position: Vector2 = hit.get("position", Vector2.ZERO)
		if hit_position.y < top_y - RECOVERY_PROBE_TOLERANCE or hit_position.y > bottom_y + RECOVERY_PROBE_TOLERANCE:
			continue
		var distance := absf(hit_position.y - bottom_y)
		if distance < best_distance:
			best_distance = distance
			best_y = hit_position.y
	return best_y

func _stance_weight(leg_name: String) -> float:
	if not _walking or _rig._anim_player == null:
		return 1.0
	var phase: float = fmod(_rig._anim_player.get_current_animation_position(), WALK_CYCLE_DURATION)
	var right_weight: float = 0.5 + 0.5 * cos(TAU * phase / WALK_CYCLE_DURATION)
	return right_weight if leg_name == "Right_Leg" else 1.0 - right_weight

func _update_terrain_following(delta: float) -> void:
	if not terrain_following_enabled or _rig.state != StickmanRig.RigState.ANIMATED or _leg_base_markers.size() != 2:
		terrain_pelvis_offset = 0.0
		return
	_rig._set_limb_mods_enabled(true)
	var center_world: Vector2 = _rig.to_global(NAV_AGENT_LOCAL_POS)
	var center_probe: Dictionary = _probe_terrain_fan(center_world, TERRAIN_CENTER_PROBE_OFFSETS)
	var center_delta: float = (center_probe["world_pos"] as Vector2).y - center_world.y if bool(center_probe.get("valid", false)) else INF
	var center_supported: bool = bool(center_probe.get("valid", false)) and absf(center_delta) <= TERRAIN_SUPPORT_RANGE
	var center_surface_y: float = (center_probe["world_pos"] as Vector2).y if bool(center_probe.get("valid", false)) else INF
	var solved: Dictionary = {}
	for leg_name: String in ["Left_Leg", "Right_Leg"]:
		var base: Marker2D = _leg_base_markers[leg_name] as Marker2D
		var target: Marker2D = _rig._get_ik_marker(leg_name)
		if base == null or target == null:
			continue
		var base_world: Vector2 = base.global_position
		var probe: Dictionary = _probe_terrain_fan(base_world, TERRAIN_FOOT_PROBE_OFFSETS)
		var probe_delta: float = (probe["world_pos"] as Vector2).y - base_world.y if bool(probe.get("valid", false)) else INF
		var within_reach: bool = absf(probe_delta) <= PELVIS_MAX_OFFSET
		if within_reach and center_supported:
			within_reach = (probe["world_pos"] as Vector2).y <= center_surface_y + TERRAIN_MAX_FOOT_SURFACE_DROP
		var stance: float = _stance_weight(leg_name)
		var contact: Dictionary = foot_contacts[leg_name]
		if stance >= 0.5 and bool(probe.get("valid", false)) and within_reach:
			if not bool(contact.get("locked", false)):
				contact = {"valid": true, "locked": true, "world_pos": probe["world_pos"], "normal": probe["normal"]}
			elif (contact.get("world_pos", base_world) as Vector2).distance_to(probe["world_pos"]) > 2.0:
				contact["world_pos"] = probe["world_pos"]
		elif stance < 0.5 or not within_reach:
			contact = {"valid": bool(probe.get("valid", false)), "locked": false}
		foot_contacts[leg_name] = contact
		var floor_world: Vector2 = contact.get("world_pos", base_world) as Vector2 if bool(contact.get("locked", false)) else probe.get("world_pos", base_world) as Vector2
		solved[leg_name] = {"base_marker": base, "floor": floor_world, "surface": probe.get("world_pos", base_world) if within_reach else base_world, "surface_valid": bool(probe.get("valid", false)) and within_reach, "weight": stance, "target": target}
	if terrain_motion == TerrainMotion.LANDING_IMPACT:
		_update_landing_impact(delta)
		return
	if terrain_motion == TerrainMotion.AIRBORNE:
		_update_airborne(delta, solved)
		return
	if not center_supported:
		_enter_airborne(_last_grounded_walk_velocity_x)
		_update_airborne(delta, solved)
		return
	terrain_pelvis_offset = lerpf(0.0, clampf(center_delta, -PELVIS_MAX_OFFSET, PELVIS_MAX_OFFSET), 1.0 - exp(-PELVIS_RESPONSE * delta))
	terrain_motion = TerrainMotion.GROUNDED
	terrain_air_velocity = Vector2.ZERO
	_resume_walk_animation()
	_rig.global_position.y += terrain_pelvis_offset
	for leg_name: String in solved:
		var item: Dictionary = solved[leg_name]
		var base_after: Vector2 = (item["base_marker"] as Marker2D).global_position
		var weight: float = float(item["weight"])
		var surface: Vector2 = item["surface"] as Vector2
		if bool(item["surface_valid"]):
			weight = maxf(weight, smoothstep(0.0, 1.0, 1.0 - clampf((surface.y - base_after.y) / TERRAIN_FOOT_GUARD_HEIGHT, 0.0, 1.0)))
		var wanted: Vector2 = base_after
		wanted.y = lerpf(base_after.y, (item["floor"] as Vector2).y, weight)
		if bool(item["surface_valid"]) and wanted.y > surface.y:
			wanted.y = surface.y
		(item["target"] as Marker2D).global_position = wanted

func _enter_airborne(horizontal_velocity: float) -> void:
	if terrain_motion != TerrainMotion.AIRBORNE:
		air_pose_base.clear()
		for marker_name: String in ["Left_Hand", "Right_Hand", "Left_Leg", "Right_Leg"]:
			var marker: Marker2D = _rig._get_ik_marker(marker_name)
			if marker != null:
				air_pose_base[marker_name] = marker.position
		_air_time = 0.0
		air_pose_weight = 0.0
		_air_entry_root_y = _rig.global_position.y
		air_fall_distance = 0.0
	terrain_motion = TerrainMotion.AIRBORNE
	terrain_air_velocity = Vector2(horizontal_velocity, 0.0)
	if _walking and _rig._anim_player != null and _rig._anim_player.is_playing():
		_rig._anim_player.pause()
		_terrain_paused_walk_animation = true
	_reset_foot_contacts()

func _update_airborne(delta: float, solved: Dictionary) -> void:
	terrain_air_velocity.x *= exp(-AIR_DRAG_X * delta)
	terrain_air_velocity.y = minf(terrain_air_velocity.y + TERRAIN_GRAVITY * delta, TERRAIN_MAX_FALL_SPEED)
	_rig.global_position += _sweep_airborne_horizontal_motion(Vector2(terrain_air_velocity.x * delta, 0.0))
	_rig.global_position.y += terrain_air_velocity.y * delta
	air_fall_distance = maxf(air_fall_distance, _rig.global_position.y - _air_entry_root_y)
	_air_time += delta
	_apply_air_pose(delta)
	var center_world: Vector2 = _rig.to_global(NAV_AGENT_LOCAL_POS)
	var center_probe: Dictionary = _probe_terrain_fan(center_world, TERRAIN_CENTER_PROBE_OFFSETS)
	var landing_delta: float = (center_probe["world_pos"] as Vector2).y - center_world.y if bool(center_probe.get("valid", false)) else INF
	if bool(center_probe.get("valid", false)) and terrain_air_velocity.y >= 0.0 and landing_delta >= -TERRAIN_SUPPORT_RANGE and landing_delta <= TERRAIN_SUPPORT_RANGE + terrain_air_velocity.y * delta:
		_rig.global_position.y += landing_delta
		var speed: float = maxf(terrain_air_velocity.y, 0.0)
		terrain_air_velocity = Vector2.ZERO
		if air_fall_distance >= HARD_LANDING_MIN_FALL_HEIGHT:
			_begin_landing(speed, center_probe, solved)
		else:
			_restore_air_pose()
			terrain_motion = TerrainMotion.GROUNDED
			_resume_walk_animation()

func _apply_air_pose(delta: float) -> void:
	if air_pose_base.is_empty():
		return
	var speed: float = maxf(terrain_air_velocity.y, 0.0)
	air_pose_weight = lerpf(air_pose_weight, smoothstep(AIR_POSE_START_SPEED, AIR_POSE_FULL_SPEED, speed), 1.0 - exp(-AIR_POSE_RESPONSE * delta))
	var forward_weight: float = smoothstep(AIR_HAND_FORWARD_START_SPEED, AIR_HAND_FORWARD_FULL_SPEED, speed)
	var phase: float = TAU * AIR_FLAIL_HZ * _air_time
	for marker_name: String in air_pose_base:
		var marker: Marker2D = _rig._get_ik_marker(marker_name)
		if marker == null:
			continue
		var base: Vector2 = air_pose_base[marker_name]
		var local_phase: float = phase + (PI if marker_name.begins_with("Right") else 0.0)
		var offset: Vector2
		if marker_name.ends_with("Hand"):
			offset = Vector2(AIR_HAND_FORWARD * forward_weight, -AIR_HAND_LIFT * air_pose_weight) + Vector2(sin(local_phase) * AIR_HAND_FLAIL_AMPLITUDE.x, cos(local_phase) * AIR_HAND_FLAIL_AMPLITUDE.y) * air_pose_weight
			marker.position = base + offset
		else:
			offset = Vector2(sin(local_phase) * AIR_FOOT_FLAIL_AMPLITUDE.x, cos(local_phase) * AIR_FOOT_FLAIL_AMPLITUDE.y)
			marker.position = base + offset * air_pose_weight

func _sweep_airborne_horizontal_motion(motion: Vector2) -> Vector2:
	if is_zero_approx(motion.x):
		return motion
	var space_state: PhysicsDirectSpaceState2D = _rig.get_world_2d().direct_space_state
	if space_state == null:
		return motion
	var safe_fraction: float = 1.0
	var excluded: Array[RID] = []
	for own_proxy: AnimatableBody2D in collision_bodies.values():
		if own_proxy != null and is_instance_valid(own_proxy):
			var own_rid: RID = own_proxy.get_rid()
			if own_rid.is_valid():
				excluded.append(own_rid)
	for proxy: AnimatableBody2D in collision_bodies.values():
		if proxy == null or not is_instance_valid(proxy):
			continue
		var shape_node: CollisionShape2D = proxy.get_node_or_null(NodePath("CollisionShape2D")) as CollisionShape2D
		if shape_node == null or shape_node.shape == null:
			continue
		var query := PhysicsShapeQueryParameters2D.new()
		query.shape = shape_node.shape
		query.transform = shape_node.global_transform
		query.motion = motion
		query.collision_mask = COLLISION_LAYER
		query.exclude = excluded
		var cast: PackedFloat32Array = space_state.cast_motion(query)
		if cast.size() >= 1:
			safe_fraction = minf(safe_fraction, cast[0])
	if safe_fraction < 0.9999:
		terrain_air_velocity.x = 0.0
	return motion * safe_fraction

func _begin_landing(_impact_speed: float, center_probe: Dictionary, _solved: Dictionary) -> void:
	terrain_motion = TerrainMotion.LANDING_IMPACT
	_landing_elapsed = 0.0
	var weight: float = clampf(inverse_lerp(HARD_LANDING_MIN_FALL_HEIGHT, HARD_LANDING_MAX_FALL_HEIGHT, air_fall_distance), 0.0, 1.0)
	landing_compression = lerpf(LANDING_COMPRESSION_MIN, LANDING_COMPRESSION_MAX, weight)
	_landing_hand_drop = lerpf(LANDING_HAND_DROP_MIN, LANDING_HAND_DROP_MAX, weight)
	var torso: Marker2D = _rig._get_ik_marker("Torso")
	if torso != null: _landing_torso_base = torso.position
	_landing_hand_start.clear(); _landing_contacts.clear()
	for hand_name: String in ["Left_Hand", "Right_Hand"]:
		var hand: Marker2D = _rig._get_ik_marker(hand_name)
		if hand != null: _landing_hand_start[hand_name] = hand.position
	var center_y: float = (center_probe.get("world_pos", _rig.global_position) as Vector2).y
	for leg_name: String in ["Left_Leg", "Right_Leg"]:
		var target: Marker2D = _rig._get_ik_marker(leg_name)
		if target == null: continue
		var base: Marker2D = _leg_base_markers.get(leg_name) as Marker2D
		var probe: Dictionary = _probe_terrain_fan(base.global_position if base != null else target.global_position, TERRAIN_FOOT_PROBE_OFFSETS)
		var contact: Vector2 = probe["world_pos"] as Vector2 if bool(probe.get("valid", false)) else Vector2(target.global_position.x, center_y)
		_landing_contacts[leg_name] = contact; target.global_position = contact

func _update_landing_impact(delta: float) -> void:
	_landing_elapsed += delta
	var total: float = LANDING_COMPRESS_DURATION + LANDING_RECOVER_DURATION
	var offset: float = landing_compression * (1.0 - pow(1.0 - clampf(_landing_elapsed / LANDING_COMPRESS_DURATION, 0.0, 1.0), 2.0)) if _landing_elapsed <= LANDING_COMPRESS_DURATION else landing_compression * cos(clampf((_landing_elapsed - LANDING_COMPRESS_DURATION) / LANDING_RECOVER_DURATION, 0.0, 1.0) * PI * 0.5)
	var torso: Marker2D = _rig._get_ik_marker("Torso")
	if torso != null: torso.position = _landing_torso_base + Vector2.DOWN * offset
	for leg_name: String in _landing_contacts:
		var target: Marker2D = _rig._get_ik_marker(leg_name)
		if target != null: target.global_position = _landing_contacts[leg_name]
	for hand_name: String in _landing_hand_start:
		var hand: Marker2D = _rig._get_ik_marker(hand_name)
		if hand != null: hand.position = (_landing_hand_start[hand_name] as Vector2).lerp(air_pose_base.get(hand_name, _landing_hand_start[hand_name]), clampf(_landing_elapsed / total, 0.0, 1.0)) + Vector2.DOWN * _landing_hand_drop * offset / maxf(landing_compression, 0.0001)
	if _landing_elapsed >= total: _finish_landing_impact()

func _finish_landing_impact() -> void:
	var torso: Marker2D = _rig._get_ik_marker("Torso")
	if torso != null: torso.position = _landing_torso_base
	_restore_air_pose(); _landing_contacts.clear(); _landing_hand_start.clear(); _landing_elapsed = 0.0; landing_compression = 0.0; _landing_hand_drop = 0.0; terrain_motion = TerrainMotion.GROUNDED
	_resume_walk_animation()

func _cancel_landing_impact(restore_pose: bool) -> void:
	if terrain_motion == TerrainMotion.LANDING_IMPACT and restore_pose:
		var torso: Marker2D = _rig._get_ik_marker("Torso")
		if torso != null: torso.position = _landing_torso_base
	_landing_contacts.clear(); _landing_hand_start.clear(); _landing_elapsed = 0.0; landing_compression = 0.0; _landing_hand_drop = 0.0

func _restore_air_pose() -> void:
	for marker_name: String in air_pose_base:
		var marker: Marker2D = _rig._get_ik_marker(marker_name)
		if marker != null: marker.position = air_pose_base[marker_name]
	air_pose_base.clear(); air_pose_weight = 0.0; _air_time = 0.0; _air_entry_root_y = 0.0; air_fall_distance = 0.0

func _resume_walk_animation() -> void:
	if _terrain_paused_walk_animation and _walking and _rig._anim_player != null:
		_rig._anim_player.play()
		_terrain_paused_walk_animation = false