extends SceneTree

## Dev tool: do the LEGS point where the character is going, and does the CHEST
## stay pointed where the player is aiming?
##
##   godot --headless --path . -s res://debug/travel_dir_check.gd -- [skin_glb]
##
## The clip library has one forward locomotion cycle and no strafe or backpedal
## clips, so nothing in the animation itself says which way the character is
## travelling. SkinnedPlayerModel._update_travel says it by yawing the hips onto
## the travel direction and unwinding that yaw up the spine.
##
## The number that matters is LEGS OFF BY: how far the stride points from the
## direction the character is actually travelling. A backpedal is achieved by
## running the cycle BACKWARDS with the hips near square, so it is scored on
## where the feet go, not on where the hips face.
##
## CHEST is reported for information only, and is not a pass/fail. Most of it is
## the clip's own torso twist — a run cycle counter-rotates the chest against the
## hips by tens of degrees twice a stride — and averaging over a stride only
## partly removes it. It is also not what keeps the gun on target: the rifle hold
## places the weapon from the SHOULDER and the aim direction and then solves the
## arms onto it, so the aim survives whatever the torso is doing. Read this
## column as "does the upper body still look square-ish", nothing stronger.

## label, strafe, forward, the direction of travel this should read as (degrees
## about the character's up axis, 0 = straight ahead)
const CASES := [
	["forward", 0.0, 1.0, 0.0],
	["strafe right", 1.0, 0.0, -90.0],
	["strafe left", -1.0, 0.0, 90.0],
	["fwd-right", 0.7, 0.7, -45.0],
	["fwd-left", -0.7, 0.7, 45.0],
	["backpedal", 0.0, -1.0, 180.0],
	["back-right", 0.7, -0.7, -135.0],
]
const SETTLE := 70
## Frames averaged at the end of each case — comfortably more than one stride.
const MEASURE_OVER := 40

var _frames := 0
var _model: SkinnedPlayerModel = null
var _probe = null
var _case := 0
var _rows: Array = []
var _hips_sum := 0.0
var _chest_sum := 0.0
var _n := 0.0


func _initialize() -> void:
	var args := OS.get_cmdline_user_args()
	var path: String = args[0] if args.size() > 0 \
		else "res://assets/characters/skins/taila.glb"
	var scene := Node3D.new()
	root.add_child(scene)
	current_scene = scene
	_model = SkinnedPlayerModel.new()
	_model.model_path = path
	scene.add_child(_model)


## Bone poses read from INSIDE the modifier pass — Godot restores them after it,
## so anything read later is the animation alone with the pose layer missing.
class PoseProbe extends SkeletonModifier3D:
	var pose: Array = []

	func _process_modification() -> void:
		var skel := get_skeleton()
		if skel == null:
			return
		pose.resize(skel.get_bone_count())
		for i in skel.get_bone_count():
			pose[i] = skel.get_bone_global_pose(i)


func _process(_delta: float) -> bool:
	_frames += 1
	if _frames < 8 or not _model.loaded:
		return false
	var skel: Skeleton3D = _model.skeleton
	if skel == null:
		return true
	if _probe == null:
		_probe = PoseProbe.new()
		_probe.name = "TravelProbe"
		skel.add_child(_probe)
		return false

	var c: Array = CASES[_case]
	# Run fast enough that the lean, which scales with speed, is fully in.
	_model.update_state("ground", 9.0, false)
	_model.set_locomotion(c[1], c[2], 0.0)
	# AVERAGED over the tail of each case, not sampled at one instant.
	#
	# A run cycle twists the torso against the hips by tens of degrees twice per
	# stride, all of it authored and correct. Reading a single frame therefore
	# measures wherever the clip happened to be, and the numbers swing far wider
	# than anything the pose layer does — a backpedal read as 50 degrees of chest
	# yaw with the hips almost square, which is the clip, not the layer.
	var phase: int = (_frames - 9) % SETTLE
	if phase >= SETTLE - MEASURE_OVER and _probe.pose.size() == skel.get_bone_count():
		_hips_sum += _yaw_of(skel, "hips")
		_chest_sum += _yaw_of(skel, "spine_top")
		_n += 1.0
	if phase != SETTLE - 1:
		return false

	if _probe.pose.size() == skel.get_bone_count():
		_rows.append([c[0], _hips_sum / _n, _chest_sum / _n,
			float(c[3]), _model.stride_reversed_debug()])
	_hips_sum = 0.0
	_chest_sum = 0.0
	_n = 0.0
	_case += 1
	if _case >= CASES.size():
		_report()
		return true
	return false


## Yaw of a bone away from its own rest orientation, in degrees, about the
## character's up axis. Measured against REST rather than against the parent, so
## it reads as "how far round from square" whatever the rig's bone axes are.
func _yaw_of(skel: Skeleton3D, which: String) -> float:
	var roles: Dictionary = _model._rig_info.get("roles", {})
	var name := ""
	if which == "hips":
		name = String(roles.get("hips", "DEF-hips"))
	else:
		var spine: Array = roles.get("spine", [])
		# The chest: the last spine bone that is not the neck or the head.
		var neck := String(roles.get("neck", ""))
		var head := String(roles.get("head", ""))
		for n in spine:
			if String(n) != neck and String(n) != head:
				name = String(n)
	var b := skel.find_bone(name)
	if b < 0:
		return 0.0
	var now: Basis = (_probe.pose[b] as Transform3D).basis
	var rest: Basis = skel.get_bone_global_rest(b).basis
	var delta := rest.inverse() * now
	# The character faces +Z; how far that axis has swung in the ground plane.
	var f: Vector3 = rest * (delta * (rest.inverse() * Vector3(0, 0, 1)))
	return rad_to_deg(atan2(f.x, f.z))


func _report() -> void:
	print("\n=== does the body say which way it is going? ===")
	print("   %-13s %9s %9s %9s   %s" % [
		"input", "hips", "chest", "want legs", "stride"])
	for r in _rows:
		# A reversed stride points the feet the opposite way to the hips.
		var legs: float = r[1] + (180.0 if r[4] else 0.0)
		var err: float = absf(wrapf(legs - r[3], -180.0, 180.0))
		print("   %-13s %+8.1f° %+8.1f° %+8.1f°   %-8s  legs off by %5.1f°%s" % [
			r[0], r[1], r[2], r[3], "reversed" if r[4] else "forward", err,
			"   CHEST OFF AIM" if absf(r[2]) > 25.0 else ""])
	print("\n   'legs off by' is how far the stride points from the travel")
	print("   direction. The hips are capped (see MAX_TRAVEL_YAW), so a pure")
	print("   sidestep is expected to fall short — a shooter keeps its aim.\n")