extends SceneTree

## Where do the hand bones actually end up relative to the grip? Renders are too coarse
## to tell a 5 cm error from a 50 cm one, so print the numbers.
##
##   Godot --headless --path game --script dev/probe_vm.gd

func _initialize() -> void:
	var main: Node = (load("res://main.tscn") as PackedScene).instantiate()
	get_root().add_child(main)
	await process_frame
	await process_frame

	var p := _find(main, "Player") as Player
	if p == null:
		print("no player"); quit(); return
	var vm := p.viewmodel
	var cam := p.camera

	for slot in [0, 2, 4]:
		p.select(slot)
		# select() defers the actual equip to the swap animation; force it for the probe
		vm.equip(p.loadout()[slot])
		await process_frame
		var w: Weapon = p.loadout()[slot]
		print("\n===== slot %d  %s" % [slot, w.display_name])
		var grip: Node3D = vm.get_node("Rig/Grip")
		print("  grip  (cam space) = ", cam.global_transform.affine_inverse() * grip.global_position)
		for side in ["ArmR", "ArmL"]:
			var arm := vm.get_node_or_null("Rig/" + side) as Node3D
			if arm == null:
				print("  %s: MISSING" % side); continue
			var skel := _skel(arm)
			var bone := "mixamorig_RightHand" if side == "ArmR" else "mixamorig_LeftHand"
			var bi := skel.find_bone(bone)
			var hand_world := skel.global_transform * skel.get_bone_global_pose(bi)
			var in_cam := cam.global_transform.affine_inverse() * hand_world
			print("  %s hand (cam space) = %s" % [side, in_cam.origin])
			# where does the forearm run? (elbow -> hand direction, in camera space)
			var fb := skel.find_bone("mixamorig_RightForeArm" if side == "ArmR" else "mixamorig_LeftForeArm")
			var fore_world := skel.global_transform * skel.get_bone_global_pose(fb)
			var fore_cam := cam.global_transform.affine_inverse() * fore_world
			print("       elbow (cam space) = %s   forearm dir = %s" % [
				fore_cam.origin, (in_cam.origin - fore_cam.origin).normalized()])
		if vm.get_node_or_null("Rig/Grip").get_child_count() > 0:
			var wn := grip.get_child(0) as Node3D
			var ab := _aabb(wn)
			print("  weapon aabb (local) pos=%s size=%s" % [ab.position, ab.size])
	quit()

func _find(n: Node, cls: String) -> Node:
	if n.get_class() == cls or (cls == "Player" and n is Player):
		return n
	for c in n.get_children():
		var r := _find(c, cls)
		if r != null:
			return r
	return null

func _skel(n: Node) -> Skeleton3D:
	if n is Skeleton3D:
		return n
	for c in n.get_children():
		var r := _skel(c)
		if r != null:
			return r
	return null

func _aabb(n: Node) -> AABB:
	var acc := AABB()
	var started := false
	for m in _meshes(n):
		var a: AABB = (m as MeshInstance3D).get_aabb()
		if not started:
			acc = a; started = true
		else:
			acc = acc.merge(a)
	return acc

func _meshes(n: Node, acc: Array = []) -> Array:
	if n is MeshInstance3D:
		acc.append(n)
	for c in n.get_children():
		_meshes(c, acc)
	return acc