extends SceneTree

## Does rotating the trigger wrist turn the HAND without taking the GUN with it?
##
## The two are welded by construction: the weapon is a child of a
## BoneAttachment3D on the trigger hand, so every degree the wrist turns swings
## the barrel the same degree off the aim line — and takes with it every control
## that could have corrected for it, because they are all expressed relative to
## that same hand. The wrist knob was therefore unusable for the one thing it
## exists for, which is aligning a hand to a gun.
##
## Both halves have to be asserted, and the second is the one that was broken:
##
##   the HAND turns      or the knob does nothing
##   the GUN does not    or the knob cannot be used
##
## Measured through a PoseProbe for the hand, because Godot restores every
## bone's local pose after the modifier pass and reading it from here would
## report the animation rather than the hold. The gun is a scene node, so its
## own global transform is the truth and no probe is needed.
##
##     godot --path . -s res://debug/wrist_gun_check.gd

const LAB := "res://debug/rig_lab.tscn"
## Large on purpose. A small twist could hide behind the tolerance of a gun that
## really was following the hand.
const TWIST := 0.5
## Degrees the barrel may wander. Not zero: the hand's rotation feeds the IK
## chain that positions the whole arm, so the shoulder and the aim line settle
## very slightly differently — but a gun still welded to the wrist would move by
## something on the order of TWIST, which is 28 degrees.
const GUN_TOLERANCE := 3.0
const HAND_MIN := 5.0

var _fails := 0
var _probe: PoseProbe = null


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 _init() -> void:
	await process_frame
	var lab: Node = load(LAB).instantiate()
	root.add_child(lab)
	for _i in 200:
		await process_frame

	var model = lab._model
	if model == null or model._pose_mod == null or model.skeleton == null:
		_expect(false, "the lab built a character holding a weapon")
		_done()
		return
	var gun := _gun(model)
	if gun == null:
		_expect(false, "the character mounted a weapon")
		_done()
		return
	_expect(true, "the lab built a character holding a weapon")

	_probe = PoseProbe.new()
	_probe.name = "WristGunProbe"
	model.skeleton.add_child(_probe)
	for _i in 10:
		await process_frame

	var hand: int = _role_bone(model, "hand.R")
	for pose in ["hip", "ads"]:
		lab._pose = 0 if pose == "hip" else 1
		for axis in 3:
			var v := Vector3.ZERO
			v[axis] = TWIST
			model.set_hold_tuning({})
			await _settle(lab, pose)
			var hand0 := _probe_rot(hand)
			var gun0: Basis = gun.global_transform.basis.orthonormalized()

			model.set_hold_tuning({"wrist_r_%s" % pose: v})
			await _settle(lab, pose)
			var hand1 := _probe_rot(hand)
			var gun1: Basis = gun.global_transform.basis.orthonormalized()

			var hand_moved := rad_to_deg(hand0.angle_to(hand1))
			var gun_moved := rad_to_deg(
				gun0.get_rotation_quaternion().angle_to(gun1.get_rotation_quaternion()))
			_expect(hand_moved >= HAND_MIN,
				"%s axis %d: the hand turned %.1f deg" % [pose, axis, hand_moved])
			_expect(gun_moved <= GUN_TOLERANCE,
				"%s axis %d: the gun stayed put (%.1f deg)" % [pose, axis, gun_moved])
	model.set_hold_tuning({})
	_done()


func _settle(lab: Node, pose: String) -> void:
	lab._model.update_state("ground", 0.0, false)
	lab._model.set_locomotion(0.0, 0.0, 1.0 if pose == "ads" else 0.0)
	for _i in 60:
		await process_frame


func _gun(model) -> Node3D:
	if model.skeleton == null:
		return null
	var attach: Node = model.skeleton.get_node_or_null("WeaponAttachment")
	if attach == null or attach.get_child_count() == 0:
		return null
	return attach.get_child(0) as Node3D


func _probe_rot(bone: int) -> Quaternion:
	if _probe == null or bone < 0 or bone >= _probe.pose.size():
		return Quaternion.IDENTITY
	var t: Transform3D = _probe.pose[bone]
	return t.basis.orthonormalized().get_rotation_quaternion()


func _role_bone(model, role: String) -> int:
	var name: String = model._rig_info.get("roles", {}).get(role, "")
	return model.skeleton.find_bone(name) if name != "" else -1


func _expect(ok: bool, what: String) -> void:
	if ok:
		print("  OK: %s" % what)
	else:
		print("  FAIL: %s" % what)
		_fails += 1


func _done() -> void:
	print("\n=== WRIST vs GUN ===\nFailures: %d" % _fails)
	quit(1 if _fails > 0 else 0)