# test_phase4b_walk.gd
# Headless regression checks for the Phase 4b walk-waypoint arrival-jitter fix
# (spec §3.4 / §5.9):
#   1. On-mesh waypoint: steering mode latches "nav" once, exactly one `arrived`
#      fires, and the rig position is unchanged for 60+ physics frames after.
#   2. Off-mesh waypoint: mode latches "direct" once, one arrive, stable.
#   3. No vertical jitter: the root's final resting Y equals the snapped target.
#
# Run with:
#   & "C:\Godot4\Godot_v4.7.1-stable_win64_console.exe" --headless --script res://tests/test_phase4b_walk.gd --path .
#
# Prints PASS/FAIL per assertion and exits 0 on all PASS, 1 on any FAIL.

extends SceneTree

const STAGE_SCENE := preload("res://scenes/sandbox_stage.tscn")
const RIG := preload("res://scripts/stickman_rig.gd")

const SETTLE_FRAMES := 60
const MAX_FRAMES := 300

var _checks := 0
var _failures := 0
var _arrived_count := 0


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


func _on_arrived(_target: Vector2) -> void:
	_arrived_count += 1


func _run() -> void:
	print("")
	print("========================================================")
	print("  PHASE 4b WALK JITTER / LATCH TEST (headless)")
	print("========================================================")

	# On-mesh waypoint (x=50 inside the 200-wide ground slab).
	await _walk_case("on-mesh", Vector2(50.0, 0.0), "nav")
	# Off-mesh waypoint (x=200 outside the slab edge at x=100).
	await _walk_case("off-mesh", Vector2(200.0, 0.0), "direct")

	print("--------------------------------------------------------")
	if _failures == 0:
		print("RESULT: ALL PASSED  (%d assertions, 0 failures)" % _checks)
		quit(0)
	else:
		print("RESULT: %d FAILURE(S) out of %d assertions" % [_failures, _checks])
		quit(1)


func _walk_case(label: String, target_feet: Vector2, expected_mode: String) -> void:
	print("")
	print("--- %s waypoint (%s) ---" % [label, target_feet])
	var stage: Node2D = STAGE_SCENE.instantiate()
	root.add_child(stage)
	var ground = stage._spawner.spawn("ground", Vector2(0, 0))
	stage._rebake_navigation()
	var rig = stage._spawner.spawn("stickman", Vector2(0, 0))
	_check(rig != null, "%s: stickman spawns" % label)
	if rig == null:
		stage.queue_free()
		await process_frame
		return
	_arrived_count = 0
	rig.arrived.connect(_on_arrived)
	rig.walk_to(target_feet, 200.0)
	_check(not rig._walk_mode_latched, "%s: mode not latched before map sync" % label)
	var frames := 0
	while not rig._walk_done and frames < MAX_FRAMES:
		await physics_frame
		frames += 1
	_check(rig._walk_done, "%s: walk finished within %d frames (used %d)" % [label, MAX_FRAMES, frames])
	_check(_arrived_count == 1, "%s: exactly one arrived signal (got %d)" % [label, _arrived_count])
	_check(rig._walk_mode_latched, "%s: mode is latched" % label)
	_check(rig._walk_mode == expected_mode,
		"%s: mode latched to '%s' (got '%s')" % [label, expected_mode, rig._walk_mode])
	# Final position equals the snapped root target (feet target + FOOT_OFFSET).
	var final_root: Vector2 = target_feet + RIG.FOOT_OFFSET
	_check(rig.global_position.distance_to(final_root) <= 0.01,
		"%s: position snapped to final root (dist=%.3f)" % [label, rig.global_position.distance_to(final_root)])
	# Body-bob settle: Torso marker restored to STAND_POSE (0,10).
	var torso = rig.get_node_or_null(NodePath("IK_Targets/Torso"))
	if torso != null:
		var stand_torso: Dictionary = RIG.STAND_POSE["Torso"]
		var expected_pos: Vector2 = stand_torso.get("pos", Vector2(0, 10))
		_check((torso as Node2D).position.distance_to(expected_pos) <= 1.0,
			"%s: Torso marker restored to standing pose (dist=%.3f)" % [label, (torso as Node2D).position.distance_to(expected_pos)])
	else:
		_check(false, "%s: IK_Targets/Torso missing" % label)
	# Post-arrival stability for 60 physics frames.
	var p0: Vector2 = rig.global_position
	var settled := true
	for i in SETTLE_FRAMES:
		await physics_frame
		if rig.global_position != p0:
			settled = false
			break
	_check(settled and rig.global_position == p0,
		"%s: position unchanged for %d frames after arrival" % [label, SETTLE_FRAMES])
	_check(not rig.is_walking(), "%s: is_walking false after arrival" % label)

	stage.queue_free()
	await process_frame


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