extends SceneTree

## Regression gate for jet-propelled wall traversal and physical grapple posing.
##
## Wall traversal must select the authored side-specific pilot performances,
## keep their source timing independent of gameplay speed, and face along the
## live wall tangent. Jets still own propulsion; the animation owns silhouette.
## Grapple must keep its authored airborne base while
## the live arm constraint lands the palm on the cable ray and body alignment
## responds to pendulum energy.

const SETTLE_FRAMES := 90
const SAMPLE_FRAMES := 30
const WALL_GLIDE_SPEED := 13.0

var _failures := 0


func _initialize() -> void:
	call_deferred("_run")


func _run() -> void:
	await process_frame
	var args := OS.get_cmdline_user_args()
	var path: String = args[0] if not args.is_empty() \
		else "res://assets/characters/skins/taila.glb"

	var scene := CharacterBody3D.new()
	root.add_child(scene)
	current_scene = scene
	var model := SkinnedPlayerModel.new()
	model.model_path = path
	scene.add_child(model)

	for _frame in 60:
		await process_frame
	if not model.loaded or model.skeleton == null:
		_expect(false, "the authored character loads")
		_done()
		return
	var foot := model._role_bone("foot.L", ["foot.L", "LeftFoot", "Left ankle"])
	var hips := model._role_bone("hips", ["hips", "Hips", "pelvis"])
	var left_hand := model._role_bone("hand.L", ["hand.L", "LeftHand", "hand_l"])
	var right_hand := model._role_bone("hand.R", ["hand.R", "RightHand", "hand_r"])
	var thigh_l := model._role_bone("thigh.L", ["thigh.L", "Left thigh"])
	var thigh_r := model._role_bone("thigh.R", ["thigh.R", "Right thigh"])
	var shin_l := model._role_bone("shin.L", ["shin.L", "Left shin"])
	var shin_r := model._role_bone("shin.R", ["shin.R", "Right shin"])
	var spine_roles: Array = model._rig_info.get("roles", {}).get("spine", [])
	var torso_chain := _torso_chain_bones(model, spine_roles)
	var lower_spine := RigRoles.find_imported_bone(
		model.skeleton, String(spine_roles[0])) \
		if not spine_roles.is_empty() else model.skeleton.find_bone(
			"DEF-spine.001")
	_expect(foot >= 0, "a foot bone resolves")
	_expect(hips >= 0 and left_hand >= 0 and right_hand >= 0,
		"hips and both hands resolve")
	if model._has_titanfall_motion_reference and thigh_l >= 0 \
			and thigh_r >= 0 and shin_l >= 0 and shin_r >= 0:
		if lower_spine >= 0:
			var lower_spine_rest_rise := _world_rest_rise(
				model, lower_spine, hips)
			for _frame in SETTLE_FRAMES:
				model.update_state("idle", 0.0, false)
				await process_frame
			var idle_spine_rise := _world_pose_rise(model, lower_spine, hips)
			_expect(idle_spine_rise > maxf(lower_spine_rest_rise * 0.65, 0.02),
				"idle keeps the lower spine above the pelvis (%.3f m)"
					% idle_spine_rise)
		var idle_horizontal_ratio_max := 0.0
		for sample in 120:
			idle_horizontal_ratio_max = maxf(idle_horizontal_ratio_max,
				_max_leg_horizontal_ratio(model, thigh_l, thigh_r, shin_l, shin_r))
			model.update_state("idle", 0.0, false)
			await process_frame
		_expect(idle_horizontal_ratio_max < 0.55,
			"idle keeps both thigh-to-knee segments mostly vertical "
				+ "(%.3f horizontal ratio)" % idle_horizontal_ratio_max)
	if model._has_titanfall_motion_reference and thigh_l >= 0 \
			and thigh_r >= 0 and shin_l >= 0 and shin_r >= 0:
		for side in [-1.0, 1.0]:
			var entry_leg_rise := await _wall_entry_leg_rise(
				model, side, thigh_l, thigh_r, shin_l, shin_r)
			_expect(entry_leg_rise < 0.05,
				"wall-run entry keeps both legs below the pelvis on side %+.0f "
					% side)

	model.set_wall_side(-1.0)
	model.set_wall_surface(Vector3.RIGHT, Vector3(-0.4, 0.0, 0.0))
	model.set_wall_glide_motion(Vector3(13.0, 0.0, 0.0))
	await _settle(model, "wall_run", WALL_GLIDE_SPEED)
	var expected_left_wall_clip := "WallRunRight" \
		if model._has_titanfall_motion_reference else "Grapple"
	_expect(model.current_clip_debug() == expected_left_wall_clip,
		"left wall traversal selects its configured authored performance")
	if model._has_titanfall_motion_reference:
		var left_torso_joint := _max_torso_joint_delta(model, torso_chain)
		_expect(left_torso_joint < 1.35,
			"left wallrun keeps a continuous authored torso (%.3f rad max joint)"
				% left_torso_joint)
	var wall_scale := float(model._anim_tree.get("parameters/loco_scale/scale"))
	_expect(absf(wall_scale - 1.0) < 0.02,
		"wall traversal keeps the authored source timing (%.3fx)"
			% wall_scale)
	var wall_foot_motion := await _sample_foot_motion(
		model, foot, "wall_run", WALL_GLIDE_SPEED)
	if model._has_titanfall_motion_reference:
		_expect(wall_foot_motion > 0.12,
			"wall traversal retains its authored propulsion cycle "
				+ "(%.3f rad motion)" % wall_foot_motion)
	else:
		_expect(wall_foot_motion < 0.08,
			"fallback wall glide holds its authored stabilization pose "
				+ "(%.3f rad motion)" % wall_foot_motion)
	var wall_forward := model.wall_glide_forward_debug()
	_expect(wall_forward.dot(Vector3.RIGHT) > 0.97,
		"wall-glide model faces along wall-tangent velocity (dot %.3f)"
			% wall_forward.dot(Vector3.RIGHT))
	model.set_wall_side(1.0)
	model.set_wall_surface(Vector3.LEFT, Vector3(0.4, 0.0, 0.0))
	await _settle(model, "wall_run", WALL_GLIDE_SPEED)
	var expected_right_wall_clip := "WallRunLeft" \
		if model._has_titanfall_motion_reference else "Grapple"
	_expect(model.current_clip_debug() == expected_right_wall_clip,
		"right wall traversal selects its configured authored performance")
	if model._has_titanfall_motion_reference:
		var right_torso_joint := _max_torso_joint_delta(model, torso_chain)
		_expect(right_torso_joint < 1.35,
			"right wallrun keeps a continuous authored torso (%.3f rad max joint)"
				% right_torso_joint)
	if model._has_titanfall_motion_reference and thigh_l >= 0 \
			and thigh_r >= 0 and shin_l >= 0 and shin_r >= 0 \
		and lower_spine >= 0:
		var right_wall_spine_rise := _world_pose_rise(
			model, lower_spine, hips)
		var lower_spine_rest_rise := _world_rest_rise(
			model, lower_spine, hips)
		_expect(right_wall_spine_rise > maxf(
			lower_spine_rest_rise * 0.65, 0.02),
			"right wallrun keeps the lower spine above the pelvis (%.3f m)"
				% right_wall_spine_rise)
		var exit_leg_rise := await _wall_exit_leg_rise(
			model, thigh_l, thigh_r, shin_l, shin_r)
		_expect(exit_leg_rise < 0.35,
			"right wallrun ground exit avoids an exaggerated knee flip")
		var jump_exit_leg_rise := await _wall_jump_exit_leg_rise(
			model, thigh_l, thigh_r, shin_l, shin_r)
		_expect(jump_exit_leg_rise < 0.05,
			"right wallrun jump hands off without a leg flip")
	_expect(not model.clip_names_debug().has("WallRun"),
		"the relabelled ground-run WallRun clip is absent")

	# The wall-glide orientation is a temporary presentation frame. It must not
	# ease back around the floor after traversal ends, and a character with an
	# authored slide entry must use that entry pose before the loop.
	model.update_state("ground", WALL_GLIDE_SPEED, false)
	await process_frame
	_expect(absf(model._motion_root.rotation.y) < 0.001,
		"wall-glide presentation yaw resets on exit")
	if model._has_titanfall_motion_reference:
		for _frame in 20:
			model.update_state("idle", 0.0, false)
			await process_frame
		var recovered_torso_joint := _max_torso_joint_delta(model, torso_chain)
		_expect(recovered_torso_joint < 0.70,
			"idle after right wallrun has no residual waist rotation "
				+ "(%.3f rad max joint)" % recovered_torso_joint)
	model.update_state("slide", WALL_GLIDE_SPEED, true)
	var slide_start_clip := String(model._resolved_clips.get("SlideStart", ""))
	if slide_start_clip != "" and slide_start_clip != \
			model._resolved_clips.get("Slide", ""):
		_expect(model.current_clip_debug() == "SlideStart",
			"slide enters through the authored SlideStart pose")
	if model._has_titanfall_motion_reference and thigh_l >= 0 \
			and thigh_r >= 0 and shin_l >= 0 and shin_r >= 0:
		var slide_entry_rise := -INF
		for _frame in 12:
			model.update_state("slide", WALL_GLIDE_SPEED, true)
			await process_frame
			var slide_leg_l := _world_bone_segment(model, thigh_l, shin_l)
			var slide_leg_r := _world_bone_segment(model, thigh_r, shin_r)
			slide_entry_rise = maxf(slide_entry_rise,
				maxf(slide_leg_l.y, slide_leg_r.y))
		_expect(slide_entry_rise < 0.05,
			"slide entry keeps both legs below the pelvis")
		model.update_state("ground", 0.0, false)
		await process_frame
		model.update_state("slide", 4.0, true)
		var slow_slide_scale := float(model._anim_tree.get(
			"parameters/loco_scale/scale"))
		model.update_state("ground", 0.0, false)
		await process_frame
		model.update_state("slide", WALL_GLIDE_SPEED, true)
		var fast_slide_scale := float(model._anim_tree.get(
			"parameters/loco_scale/scale"))
		_expect(fast_slide_scale > slow_slide_scale + 0.25,
			"slide entry playback follows momentum (%.2fx -> %.2fx)"
				% [slow_slide_scale, fast_slide_scale])

	# The imported performance must own the unarmed silhouette. A target rig's
	# crossed/T-pose rest arms used to survive every clip because only relative
	# bone rotations were copied.
	model.set_weapon("")
	model.set_locomotion(0.0, 0.0, 0.0)
	await _settle(model, "idle", 0.0)
	var left_shoulder := model._role_bone(
		"shoulder.L", ["shoulder.L", "LeftShoulder", "clavicle_l"])
	var right_shoulder := model._role_bone(
		"shoulder.R", ["shoulder.R", "RightShoulder", "clavicle_r"])
	var skeleton_basis := model.skeleton.global_transform.basis
	var idle_drop := minf(
		(skeleton_basis * (model.skeleton.get_bone_global_pose(
			left_shoulder).origin - model.skeleton.get_bone_global_pose(
			left_hand).origin)).y,
		(skeleton_basis * (model.skeleton.get_bone_global_pose(
			right_shoulder).origin - model.skeleton.get_bone_global_pose(
			right_hand).origin)).y,
	)
	_expect(idle_drop > 0.14,
		"unarmed idle hands hang below the shoulders (drop %.3f m)"
			% idle_drop)

	# A real backward capture turns/leans the whole body. The old fabricated
	# version rotated only hips and legs by 180 degrees, leaving the first
	# spine joint facing forward and creating an abdominal corkscrew.
	model.set_locomotion(0.0, -1.0, 0.0)
	await _settle(model, "ground", 8.25)
	var spine_names: Array = model._rig_info.get("roles", {}).get("spine", [])
	var first_spine := RigRoles.find_imported_bone(
		model.skeleton, String(spine_names[0])) \
		if not spine_names.is_empty() else -1
	_expect(first_spine >= 0, "the first spine bone resolves")
	if first_spine >= 0:
		var hips_pose := model.skeleton.get_bone_global_pose(hips) \
			.basis.get_rotation_quaternion()
		var hips_rest := model.skeleton.get_bone_global_rest(hips) \
			.basis.get_rotation_quaternion()
		var spine_pose := model.skeleton.get_bone_global_pose(first_spine) \
			.basis.get_rotation_quaternion()
		var spine_rest := model.skeleton.get_bone_global_rest(first_spine) \
			.basis.get_rotation_quaternion()
		var hips_delta := hips_pose * hips_rest.inverse()
		var spine_delta := spine_pose * spine_rest.inverse()
		var abdomen_twist := hips_delta.angle_to(spine_delta)
		# The Titanfall combat-backpedal deliberately keeps the weapon-facing
		# chest counter-rotated over the travelling hips.  The old broken capture
		# was a full 180-degree lower-body reversal; preserve the authored turn
		# while still rejecting that corkscrew.
		_expect(abdomen_twist < 2.25,
			"backward capture keeps the authored torso counter-turn (%.3f rad)"
				% abdomen_twist)

	var anchor := model.global_position + Vector3(3.0, 7.0, -6.0)
	await _settle_grapple(model, anchor, Vector3(1.0, 0.0, 0.0))
	_expect(model.current_clip_debug() == "Grapple",
		"grapple selects the authored Grapple clip")
	var grapple_scale := float(model._anim_tree.get("parameters/loco_scale/scale"))
	_expect(absf(grapple_scale - 1.0) < 0.001,
		"grapple holds its authored pose at 1.0x")
	var grapple_foot_motion := await _sample_grapple_foot_motion(
		model, foot, anchor, Vector3(1.0, 0.0, 0.0))
	_expect(grapple_foot_motion < 0.06,
		"grapple legs stay held instead of swimming (%.3f rad)"
			% grapple_foot_motion)

	var low: Dictionary = model.grapple_debug()
	print("  grapple slow debug: ", low)
	_expect(float(low.get("line_error", 1.0)) < 0.035,
		"slow grapple palm lies on the cable ray (%.1f mm error)"
			% (float(low.get("line_error", 1.0)) * 1000.0))
	_expect(float(low.get("alignment", 1.0)) < 0.20,
		"slow grapple hangs under gravity (alignment %.3f)"
			% float(low.get("alignment", 1.0)))
	var low_body: Quaternion = low.get(
		"body_rotation", Quaternion.IDENTITY)

	await _settle_grapple(model, anchor, Vector3(18.0, 2.0, 4.0))
	var medium: Dictionary = model.grapple_debug()
	print("  grapple medium debug: ", medium)
	_expect(float(medium.get("alignment", 1.0)) < 0.60,
		"medium grapple still reads as hanging (alignment %.3f)"
			% float(medium.get("alignment", 1.0)))

	await _settle_grapple(model, anchor, Vector3(32.0, 4.0, 8.0))
	var fast: Dictionary = model.grapple_debug()
	print("  grapple high-speed debug: ", fast)
	_expect(float(fast.get("line_error", 1.0)) < 0.035,
		"high-speed grapple palm stays on the cable ray (%.1f mm error)"
			% (float(fast.get("line_error", 1.0)) * 1000.0))
	_expect(float(fast.get("alignment", 0.0)) > 0.85,
		"only high-speed grapple aligns the body (alignment %.3f)"
			% float(fast.get("alignment", 0.0)))
	var fast_body: Quaternion = fast.get(
		"body_rotation", Quaternion.IDENTITY)
	_expect(low_body.angle_to(fast_body) > 0.20,
		"body orientation physically changes with swing energy (%.3f rad)"
			% low_body.angle_to(fast_body))

	model.queue_free()
	_done()


func _settle(model: SkinnedPlayerModel, state: String, speed: float) -> void:
	for _frame in SETTLE_FRAMES:
		model.update_state(state, speed, false)
		await process_frame


func _sample_foot_motion(
		model: SkinnedPlayerModel,
		foot: int,
		state: String,
		speed: float,
) -> float:
	if foot < 0:
		return 0.0
	var start := model.skeleton.get_bone_pose_rotation(foot)
	var greatest := 0.0
	for _frame in SAMPLE_FRAMES:
		model.update_state(state, speed, false)
		await process_frame
		greatest = maxf(greatest, start.angle_to(
			model.skeleton.get_bone_pose_rotation(foot)))
	return greatest


func _torso_chain_bones(model: SkinnedPlayerModel, spine_roles: Array) -> Array:
	var role_data: Dictionary = model._rig_info.get("roles", {})
	var neck_name := String(role_data.get("neck", ""))
	var head_name := String(role_data.get("head", ""))
	var chain := [model._role_bone("hips", ["hips", "Hips", "pelvis"])]
	for role in spine_roles:
		var bone_name := String(role)
		if bone_name == neck_name or bone_name == head_name:
			continue
		chain.append(RigRoles.find_imported_bone(model.skeleton, bone_name))
	return chain


func _max_torso_joint_delta(model: SkinnedPlayerModel, chain: Array) -> float:
	var greatest := 0.0
	for index in range(1, chain.size()):
		var parent: int = int(chain[index - 1])
		var child: int = int(chain[index])
		if parent < 0 or child < 0:
			continue
		var posed_relative := model.skeleton.get_bone_global_pose(parent) \
			.basis.inverse() * model.skeleton.get_bone_global_pose(child).basis
		var rest_relative := model.skeleton.get_bone_global_rest(parent) \
			.basis.inverse() * model.skeleton.get_bone_global_rest(child).basis
		greatest = maxf(greatest,
			posed_relative.get_rotation_quaternion().angle_to(
				rest_relative.get_rotation_quaternion()))
	return greatest


func _max_leg_horizontal_ratio(model: SkinnedPlayerModel,
		thigh_l: int, thigh_r: int, shin_l: int, shin_r: int) -> float:
	var greatest := 0.0
	for pair in [[thigh_l, shin_l], [thigh_r, shin_r]]:
		var segment := _world_bone_segment(
			model, int(pair[0]), int(pair[1]))
		if segment.length() > 0.0001:
			greatest = maxf(greatest,
				Vector2(segment.x, segment.z).length() / segment.length())
	return greatest


func _world_bone_segment(model: SkinnedPlayerModel,
		from_bone: int, to_bone: int) -> Vector3:
	var local_segment := model.skeleton.get_bone_global_pose(to_bone).origin \
		- model.skeleton.get_bone_global_pose(from_bone).origin
	return model.skeleton.global_transform.basis * local_segment


func _world_pose_rise(model: SkinnedPlayerModel, top: int, hips: int) -> float:
	var basis := model.skeleton.global_transform
	var top_world := basis * model.skeleton.get_bone_global_pose(top).origin
	var hips_world := basis * model.skeleton.get_bone_global_pose(hips).origin
	return top_world.y - hips_world.y


func _world_rest_rise(model: SkinnedPlayerModel, top: int, hips: int) -> float:
	var basis := model.skeleton.global_transform
	var top_world := basis * model.skeleton.get_bone_global_rest(top).origin
	var hips_world := basis * model.skeleton.get_bone_global_rest(hips).origin
	return top_world.y - hips_world.y


func _settle_grapple(model: SkinnedPlayerModel, anchor: Vector3,
		velocity: Vector3) -> void:
	for _frame in SETTLE_FRAMES:
		model.update_state("grapple", velocity.length(), false)
		model.set_grapple_target(anchor, velocity)
		await process_frame


func _sample_grapple_foot_motion(
		model: SkinnedPlayerModel,
		foot: int,
		anchor: Vector3,
		velocity: Vector3,
) -> float:
	if foot < 0:
		return 0.0
	var start := model.skeleton.get_bone_pose_rotation(foot)
	var greatest := 0.0
	for _frame in SAMPLE_FRAMES:
		model.update_state("grapple", velocity.length(), false)
		model.set_grapple_target(anchor, velocity)
		await process_frame
		greatest = maxf(greatest, start.angle_to(
			model.skeleton.get_bone_pose_rotation(foot)))
	return greatest


func _wall_entry_leg_rise(model: SkinnedPlayerModel, side: float,
		thigh_l: int, thigh_r: int, shin_l: int, shin_r: int) -> float:
	# Enter from the airborne pose so this exercises the exact full-body
	# transition that used to rotate the right-wall waist and legs through
	# vertical before the authored loop settled.
	model.set_wall_side(side)
	for _frame in 24:
		model.update_state("air", 4.0, false)
		await process_frame
	var greatest_rise := -INF
	for _frame in 20:
		model.update_state("wall_run", WALL_GLIDE_SPEED, false)
		await process_frame
		var leg_l := _world_bone_segment(model, thigh_l, shin_l)
		var leg_r := _world_bone_segment(model, thigh_r, shin_r)
		greatest_rise = maxf(greatest_rise, maxf(leg_l.y, leg_r.y))
	return greatest_rise


func _wall_exit_leg_rise(model: SkinnedPlayerModel,
		thigh_l: int, thigh_r: int, shin_l: int, shin_r: int) -> float:
	# Start on the problematic right-wall performance, then leave into the
	# ground locomotion state and measure the transition frames.
	model.set_wall_side(1.0)
	for _frame in SETTLE_FRAMES:
		model.update_state("wall_run", WALL_GLIDE_SPEED, false)
		await process_frame
	var greatest_rise := -INF
	for _frame in 20:
		model.update_state("ground", WALL_GLIDE_SPEED, false)
		await process_frame
		var leg_l := _world_bone_segment(model, thigh_l, shin_l)
		var leg_r := _world_bone_segment(model, thigh_r, shin_r)
		greatest_rise = maxf(greatest_rise, maxf(leg_l.y, leg_r.y))
	return greatest_rise


func _wall_jump_exit_leg_rise(model: SkinnedPlayerModel,
		thigh_l: int, thigh_r: int, shin_l: int, shin_r: int) -> float:
	# The real controller reports a positive parent velocity on the first frame
	# after a wall jump, so this exercises WallRun -> Jump rather than Fall.
	model.set_wall_side(1.0)
	for _frame in SETTLE_FRAMES:
		model.update_state("wall_run", WALL_GLIDE_SPEED, false)
		await process_frame
	var body := model.get_parent() as CharacterBody3D
	var greatest_rise := -INF
	for _frame in 20:
		if body:
			body.velocity.y = 8.0
		model.update_state("air", WALL_GLIDE_SPEED, false)
		await process_frame
		var leg_l := _world_bone_segment(model, thigh_l, shin_l)
		var leg_r := _world_bone_segment(model, thigh_r, shin_r)
		greatest_rise = maxf(greatest_rise, maxf(leg_l.y, leg_r.y))
	return greatest_rise


func _expect(ok: bool, description: String) -> void:
	if ok:
		print("  OK: ", description)
	else:
		printerr("  FAIL: ", description)
		_failures += 1


func _done() -> void:
	print("\n=== AUTHORED TRAVERSAL SUMMARY ===")
	print("Failures: %d" % _failures)
	quit(1 if _failures > 0 else 0)