extends SceneTree

const STAGE_SCENE := preload("res://scenes/stage/sandbox_stage.tscn")
const WALK_PHASES: Array[float] = [0.0, 0.08, 0.16, 0.24, 0.32, 0.40, 0.48, 0.56, 0.64, 0.72]
const SETTLE_FRAMES := 30

var _checks := 0
var _failures := 0


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


func _run() -> void:
	for profile: int in [StickmanRig.FacingProfile.RIGHT, StickmanRig.FacingProfile.LEFT]:
		for walk_phase: float in WALK_PHASES:
			await _check_handoff(profile, walk_phase)
	if _failures == 0:
		print("RESULT: ALL PASSED (%d assertions)" % _checks)
		quit(0)
	else:
		print("RESULT: %d FAILURE(S) out of %d assertions" % [_failures, _checks])
		quit(1)


func _check_handoff(profile: int, walk_phase: float) -> void:
	var stage := STAGE_SCENE.instantiate() as Node2D
	root.add_child(stage)
	stage._snap_enabled = false
	var rig: StickmanRig = stage._spawner.spawn("stickman", Vector2.ZERO)
	rig.auto_recover = false
	var destination_x := 500.0 if profile == StickmanRig.FacingProfile.RIGHT else -500.0
	rig.walk_to(Vector2(destination_x, 0.0))
	var animation := rig.get_node("AnimationPlayer") as AnimationPlayer
	animation.seek(walk_phase, true)
	await process_frame
	await physics_frame
	var label := "%s @ %.2fs" % [StickmanRig.FacingProfile.keys()[profile], walk_phase]
	_check(rig.get_facing_profile() == profile, "%s keeps its facing profile before ragdoll" % label)
	rig.set_ragdoll(true)
	var ragdoll := rig.get_node("RigComponents/Ragdoll") as StickmanRagdoll
	var expected_side := -1.0 if profile == StickmanRig.FacingProfile.RIGHT else 1.0
	_check(_both_legs_match(_ragdoll_leg_sides(ragdoll), expected_side),
		"%s enters ragdoll with both knees on the profile side" % label)
	for frame in SETTLE_FRAMES:
		await physics_frame
		await process_frame
	# SceneTree.process_frame is emitted before Node._process callbacks. Wait one
	# more idle frame so the rendered pose includes the post-physics knee guard.
	await process_frame
	var settled_sides := _ragdoll_leg_sides(ragdoll)
	_check(_both_legs_match(settled_sides, expected_side),
		"%s keeps both knees on the profile side for %d frames (sides %s)"
			% [label, SETTLE_FRAMES, str(settled_sides)])
	_check(rig.get_facing_profile() == profile, "%s never changes facing profile" % label)
	stage.queue_free()
	await process_frame


func _ragdoll_leg_sides(ragdoll: StickmanRagdoll) -> Dictionary:
	var bodies := ragdoll.get_bodies()
	var result := {}
	for prefix: String in ["left", "right"]:
		var upper := bodies.get(prefix + "_upper_leg") as RigidBody2D
		var lower := bodies.get(prefix + "_lower_leg") as RigidBody2D
		var upper_half := float(upper.get_meta("half_height", 0.0))
		var lower_half := float(lower.get_meta("half_height", 0.0))
		var upper_dir := Vector2.from_angle(upper.global_rotation)
		var lower_dir := Vector2.from_angle(lower.global_rotation)
		var hip := upper.global_position - upper_dir * upper_half
		var knee := upper.global_position + upper_dir * upper_half
		var ankle := lower.global_position + lower_dir * lower_half
		result["LeftLeg" if prefix == "left" else "RightLeg"] = \
			signf((ankle - hip).cross(knee - hip))
	return result


func _both_legs_match(sides: Dictionary, expected_side: float) -> bool:
	return float(sides.get("LeftLeg", 0.0)) == expected_side \
		and float(sides.get("RightLeg", 0.0)) == expected_side


func _check(condition: bool, label: String) -> void:
	_checks += 1
	if condition:
		print("PASS: %s" % label)
	else:
		_failures += 1
		push_error("FAIL: %s" % label)