extends SceneTree

## Dev tool: how well do the sidecar's leg capsules actually enclose the leg?
##
##   godot --headless --path . -s res://debug/leg_radius_check.gd -- [skin_glb]
##
## tools/retarget.py sizes each capsule from the MEDIAN distance of the limb's
## own vertices, which by construction leaves half the leg's surface outside the
## collider. Cloth is then pushed out to a shape narrower than the leg it is
## meant to clear, so the solver reports the panel as clear while the thigh is
## visibly through it in the render — the metric and the eye disagree, and the
## eye is right.
##
## Prints, per capsule end, the percentile spread of the real vertex distances
## next to the radius actually shipped.

const PCTS := [0.5, 0.75, 0.85, 0.95, 1.0]

var _frames := 0
var _model: SkinnedPlayerModel = null


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)


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
	var info = JSON.parse_string(FileAccess.get_file_as_string(
		_model.model_path.get_basename() + ".rig.json"))
	if typeof(info) != TYPE_DICTIONARY:
		print("no sidecar")
		return true

	# Every vertex, tagged with the bone that dominates it.
	var owned := {}   # bone index -> PackedVector3Array of rest positions
	for mi in _model.find_children("*", "MeshInstance3D", true, false):
		if mi.mesh == null or mi.skin == null:
			continue
		var skin: Skin = mi.skin
		var bone_of := {}
		for b in skin.get_bind_count():
			var bi := skin.get_bind_bone(b)
			if bi < 0:
				bi = skel.find_bone(skin.get_bind_name(b))
			bone_of[b] = bi
		for s in range(mi.mesh.get_surface_count()):
			var arrays: Array = mi.mesh.surface_get_arrays(s)
			var verts: PackedVector3Array = arrays[Mesh.ARRAY_VERTEX]
			var bones: PackedInt32Array = arrays[Mesh.ARRAY_BONES]
			var weights: PackedFloat32Array = arrays[Mesh.ARRAY_WEIGHTS]
			if bones.is_empty() or verts.is_empty():
				continue
			var per: int = bones.size() / verts.size()
			for v in verts.size():
				var bw := 0.0
				var bind := -1
				var q := Vector3.ZERO
				for k in per:
					var w: float = weights[v * per + k]
					var bi: int = bone_of[bones[v * per + k]]
					if bi < 0 or w <= 0.0:
						continue
					q += (skel.get_bone_global_rest(bi)
						* skin.get_bind_pose(bones[v * per + k]) * verts[v]) * w
					if w > bw:
						bw = w
						bind = bi
				if bind < 0 or bw < 0.5:
					continue
				if not owned.has(bind):
					owned[bind] = PackedVector3Array()
				owned[bind].append(q)

	for c in info.get("colliders", []):
		var a_i := skel.find_bone(String(c.get("bone", "")))
		var b_i := skel.find_bone(String(c.get("child", "")))
		if a_i < 0 or b_i < 0:
			continue
		var a := skel.get_bone_global_rest(a_i).origin
		var b := skel.get_bone_global_rest(b_i).origin
		var ab := b - a
		var d2 := ab.length_squared()
		# The limb is this bone plus any twist segment hanging off it — the same
		# grouping tools/retarget.py uses when it sizes the capsule.
		var base := String(c.get("bone", ""))
		var pts := PackedVector3Array()
		for bi in owned:
			var n := skel.get_bone_name(bi)
			if n == base or n.begins_with(base + "."):
				pts.append_array(owned[bi])
		# Per-tenth of the limb, so the real taper is visible instead of two
		# lumps. The head band is where a thigh stops being a thigh and becomes
		# the hip, and that is exactly the band a two-point capsule has to guess.
		var bands: Array = []
		for _b in 10:
			bands.append(PackedFloat32Array())
		for p in pts:
			var t: float = 0.0 if d2 < 0.000001 else clampf((p - a).dot(ab) / d2, 0.0, 1.0)
			bands[clampi(int(t * 10.0), 0, 9)].append(p.distance_to(a + ab * t))
		var rh := float(c.get("radius_head", 0.0))
		var rt := float(c.get("radius_tail", 0.0))
		print("%s  (%d verts)   shipped head %.4f tail %.4f" % [
			base, pts.size(), rh, rt])
		for band in 10:
			var v: PackedFloat32Array = bands[band]
			if v.is_empty():
				continue
			v.sort()
			var i: int = clampi(int(0.88 * (v.size() - 1)), 0, v.size() - 1)
			var mid: int = v.size() / 2
			var t := (float(band) + 0.5) / 10.0
			print("   t %.2f  n%-5d p50 %.4f  p88 %.4f  p100 %.4f   capsule %.4f" % [
				t, v.size(), v[mid], v[i], v[v.size() - 1], lerpf(rh, rt, t)])
	return true


func _spread(v: PackedFloat32Array) -> String:
	if v.is_empty():
		return "(none)"
	var out := ""
	for p in PCTS:
		var i: int = clampi(int(p * (v.size() - 1)), 0, v.size() - 1)
		out += "p%02d %.4f  " % [int(p * 100.0), v[i]]
	return out