extends SceneTree

## Dev tool: does the LEG actually poke through the CLOTH?
##
##   godot --headless --path . -s res://debug/cloth_clip_check.gd -- [skin_glb]
##
## Skins every cloth vertex itself over a sweep of movement states and measures
## how far each one ends up INSIDE the leg capsules from <model>.rig.json.
##
## This exists because debug/cloth_settle_check.gd measures the wrong thing for
## this question. That one reports how far a cloth BONE penetrates, which came
## back at about a millimetre while the thigh was still visibly through the
## skirt in almost every animation — because a skirt panel is a wide sheet and
## its bone is a single stick from the waist. Keeping the stick out of the leg
## says nothing about the hundreds of vertices hanging off it.
##
## Reports per surface, worst over the sweep:
##   DEPTH   how far the deepest vertex sits inside a capsule (metres)
##   COUNT   how many vertices are inside at that worst moment
##
## THE POSE IS READ FROM INSIDE THE MODIFIER PASS, from an observer
## SkeletonModifier3D added after SpringBones. It has to be. Godot restores every
## bone's local pose once the modifier pass is over, so a reader that calls
## force_update_all_bone_transforms() afterwards recomputes the global poses from
## the ANIMATION ALONE and never sees a single thing the cloth solver did. This
## tool did exactly that, and reported the same ~95 mm whether the collision was
## fully enabled or commented out — which is how the mistake was found.

## state, speed
## Idle FIRST and again LAST. A number taken from the state that happens to
## follow a dash is measuring the garment settling, not the garment at rest, and
## the two want opposite fixes — the sweep used to end on idle and reported the
## recovery as an idle failure.
const SWEEP := [["ground", 0.0], ["ground", 3.0], ["ground", 9.0], ["air", 6.0],
	["air", -8.0], ["slide", 10.0], ["dash", 14.0], ["ground", 0.0]]
const FRAMES_PER_STATE := 60

var _frames := 0
var _model: SkinnedPlayerModel = null
var _caps: Array = []          # [bone_a, bone_b, r_head, r_tail]
var _worst := {}
var _worst_n := {}
var _cloth_bones := {}         # skin bind index sets are per surface; see below
var _driver := {}              # mesh -> bone dominating its deepest vertex
var _rest := {}                # "mesh/surface" -> per-vertex rest clearance
var _key := ""
var _spring = null
## Full weight list of each surface's deepest vertex. A cloth solver can only
## move a vertex the CLOTH drives — one that is half-weighted to a thigh follows
## that thigh however well the garment is simulated, so "how much of this vertex
## does the skirt actually own" has to be part of the report.
var _mix := {}
## Worst phase of the sweep per surface, so a failure points at a movement state.
var _phase_of := {}
var _phase := 0
## bone name -> deepest contact the SOLVER reported on it over the sweep.
var _saw := {}
var _probe: PoseProbe = null
## bone name -> overlap still left once the relaxation had converged.
var _res := {}
var _per_phase := {}
var _per_phase_n := {}


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 _load_caps(skel: Skeleton3D, path: String) -> void:
	var side := path.get_basename() + ".rig.json"
	if not FileAccess.file_exists(side):
		print("no sidecar — nothing to check against")
		return
	var info = JSON.parse_string(FileAccess.get_file_as_string(side))
	if typeof(info) != TYPE_DICTIONARY:
		return
	for c in info.get("colliders", []):
		var a := skel.find_bone(String(c.get("bone", "")))
		var b := skel.find_bone(String(c.get("child", "")))
		if a < 0 or b < 0:
			continue
		var tail := float(c.get("radius_tail", c.get("radius", 0.1)))
		_caps.append([a, b, float(c.get("radius_head", tail)), tail,
			float(c.get("from", 0.0))])
	for c in info.get("chains", []):
		for n in c.get("bones", []):
			var i := skel.find_bone(String(n))
			if i >= 0:
				_cloth_bones[i] = true
	print("checking %d cloth bones against %d leg capsules" % [
		_cloth_bones.size(), _caps.size()])


func _process(_delta: float) -> bool:
	_frames += 1
	if _frames < 8:
		return false
	var skel: Skeleton3D = _model.skeleton
	if skel == null:
		return true
	if _caps.is_empty() and _cloth_bones.is_empty():
		_load_caps(skel, _model.model_path)
		if _caps.is_empty():
			return true
	if _spring == null:
		# Headless runs uncapped, so the engine delta is sub-millisecond and the
		# solver integrates almost nothing. Pin it to a real frame so the sweep
		# measures cloth in motion rather than cloth held at its rest pose.
		_spring = skel.get_node_or_null("SpringBones")
		if _spring:
			_spring.fixed_delta = 1.0 / 60.0
		_probe = PoseProbe.new()
		_probe.name = "ClipProbe"
		skel.add_child(_probe)   # AFTER SpringBones, so it sees the final pose
		return false
	if _rest.is_empty():
		# Baseline first: a skirt legitimately drapes INSIDE the thigh capsule,
		# so absolute depth says nothing. What matters is the leg getting closer
		# to a piece of cloth than the artist modelled it.
		_capture_rest(skel)
		return false
	var phase: int = clampi((_frames - 8) / FRAMES_PER_STATE, 0, SWEEP.size() - 1)
	_phase = phase
	_model.update_state(SWEEP[phase][0], SWEEP[phase][1], false)
	_model.set_locomotion(0.0, 1.0, 0.0)
	_measure(skel)
	# What the SOLVER thinks is happening, alongside what the mesh is doing. If
	# a bone's vertices are deep inside a leg while its own contact report is
	# near zero, the solver is not blind by tuning — it is not looking at the
	# geometry that is clipping.
	if _spring:
		var rep: Dictionary = _spring.debug_hit_report()
		for b in rep:
			_saw[skel.get_bone_name(b)] = maxf(_saw.get(skel.get_bone_name(b), 0.0),
				float(rep[b]))
		var res: Dictionary = _spring.debug_residual_report()
		for b in res:
			_res[skel.get_bone_name(b)] = maxf(_res.get(skel.get_bone_name(b), 0.0),
				float(res[b]))
	if _frames > 8 + FRAMES_PER_STATE * SWEEP.size():
		_report()
		return true
	return false


## Clearance of every cloth vertex to the legs in the REST pose.
func _capture_rest(skel: Skeleton3D) -> void:
	var segs: Array = []
	for c in _caps:
		# The `from` offset MATTERS. SpringBones starts a limb capsule 10% down
		# the bone because the top of a thigh is hip, buried inside the body the
		# skirt hangs from — see tools/retarget.py::_leg_colliders. Measuring
		# against the full bone tests a band the solver is deliberately not
		# defending and reports it as clipping that no tuning can ever fix.
		var ra: Vector3 = skel.get_bone_global_rest(c[0]).origin
		var rb: Vector3 = skel.get_bone_global_rest(c[1]).origin
		segs.append([ra.lerp(rb, c[4]), rb, c[2], c[3]])
	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()
			var out := PackedFloat32Array()
			out.resize(verts.size())
			for v in verts.size():
				var q := Vector3.ZERO
				for k in per:
					var w: float = weights[v * per + k]
					if w <= 0.0:
						continue
					var bi: int = bone_of[bones[v * per + k]]
					if bi < 0:
						continue
					q += (skel.get_bone_global_rest(bi) * skin.get_bind_pose(bones[v * per + k]) * verts[v]) * w
				out[v] = _clearance(q, segs)
			_rest["%s/%d" % [mi.name, s]] = out


## Which capsule the last _clearance() call picked. Reported for the deepest
## vertex, because "inside a leg" and "inside the waist lid" are different
## failures with different fixes and the bare number cannot tell them apart.
var _which := -1

## Distance from the nearest capsule SURFACE (negative = inside).
func _clearance(p: Vector3, segs: Array) -> float:
	var best := INF
	var idx := 0
	for s in segs:
		var a: Vector3 = s[0]
		var ab: Vector3 = s[1] - a
		var d2: float = ab.length_squared()
		var t: float = 0.0 if d2 < 0.000001 else clampf((p - a).dot(ab) / d2, 0.0, 1.0)
		var r: float = lerpf(s[2], s[3], t)
		var d := p.distance_to(a + ab * t) - r
		if d < best:
			best = d
			_which = idx
		idx += 1
	return best


## Snapshot of every bone's global pose, taken INSIDE the modifier pass. See the
## header: read any later and the cloth solver's work is already gone.
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 _measure(skel: Skeleton3D) -> void:
	if _probe == null or _probe.pose.size() != skel.get_bone_count():
		return
	var segs: Array = []
	for c in _caps:
		var pa: Vector3 = (_probe.pose[c[0]] as Transform3D).origin
		var pb: Vector3 = (_probe.pose[c[1]] as Transform3D).origin
		segs.append([pa.lerp(pb, c[4]), pb, c[2], c[3]])

	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()
			var deepest := 0.0
			var count := 0
			var deep_v := -1
			var deep_cap := -1
			for v in verts.size():
				# Only vertices the CLOTH actually drives — the body's own legs
				# are inside these capsules by definition.
				var is_cloth := false
				var q := Vector3.ZERO
				for k in per:
					var w: float = weights[v * per + k]
					if w <= 0.0:
						continue
					var bind: int = bones[v * per + k]
					var bi: int = bone_of[bind]
					if bi < 0:
						continue
					if _cloth_bones.has(bi) and w > 0.5:
						is_cloth = true
					q += ((_probe.pose[bi] as Transform3D) * skin.get_bind_pose(bind) * verts[v]) * w
				if not is_cloth:
					continue
				var rest_arr: PackedFloat32Array = _rest.get("%s/%d" % [mi.name, s], PackedFloat32Array())
				if v >= rest_arr.size():
					continue
				# How far INSIDE a leg this piece of cloth now is, over and above
				# however far inside the artist modelled it.
				#
				# Not "how much closer the leg got": a hem 200 mm clear of a shin
				# legitimately comes 180 mm closer when the leg kicks out in a
				# slide, and counting that as a failure buried the real clipping
				# under motion the character is supposed to have. Only cloth that
				# is actually within the capsule can be showing a leg through it.
				var inside := -_clearance(q, segs)
				var hit := _which
				if inside <= 0.0:
					continue
				var d := inside - maxf(-rest_arr[v], 0.0)
				if d > 0.0:
					count += 1
					if d > deepest:
						deepest = d
						deep_v = v
						deep_cap = hit
			if deepest <= 0.0:
				continue
			# Per state as well as overall: one worst number over a whole sweep
			# hides which movement actually breaks, and it moves to a different
			# state after every change, which reads as "no progress" when a real
			# failure has in fact been fixed and a smaller one exposed.
			var pk := "%d" % _phase
			if deepest > _per_phase.get(pk, 0.0):
				_per_phase[pk] = deepest
				_per_phase_n[pk] = count
			var key: String = "%s/%s" % [mi.name, mi.mesh.surface_get_name(s)]
			if deepest > _worst.get(key, 0.0):
				_worst[key] = deepest
				_worst_n[key] = count
				_phase_of[key] = "%s@%.0f in %s" % [SWEEP[_phase][0],
					SWEEP[_phase][1],
					skel.get_bone_name(_caps[deep_cap][0]) if deep_cap >= 0 else "?"]
				# EVERY bone driving the deepest vertex, not just the strongest.
				# A solver can only move what the cloth owns: a vertex half
				# weighted to a thigh follows that thigh however well the garment
				# is simulated, and no amount of solver work will change it.
				var mix: Array = []
				var best := 0.0
				var bn := -1
				for k in per:
					var w: float = weights[deep_v * per + k]
					if w <= 0.001:
						continue
					var bi: int = bone_of[bones[deep_v * per + k]]
					mix.append("%s=%.2f" % [
						skel.get_bone_name(bi) if bi >= 0 else "?", w])
					if w > best:
						best = w
						bn = bi
				_mix[key] = " ".join(mix)
				_driver[mi.name] = "%s w=%.2f" % [
					skel.get_bone_name(bn) if bn >= 0 else "?", best]

	# _model.set_locomotion is enough to keep the pose layer fed.


## How far inside the nearest leg capsule this point is (0 if clear).
func _penetration(p: Vector3, segs: Array) -> float:
	var worst := 0.0
	for s in segs:
		var a: Vector3 = s[0]
		var ab: Vector3 = s[1] - a
		var d2: float = ab.length_squared()
		var t: float = 0.0 if d2 < 0.000001 else clampf((p - a).dot(ab) / d2, 0.0, 1.0)
		var r: float = lerpf(s[2], s[3], t)
		worst = maxf(worst, r - p.distance_to(a + ab * t))
	return worst


func _report() -> void:
	print("\n=== worst LEG-INSIDE-CLOTH penetration over the sweep ===")
	if _worst.is_empty():
		print("   none — no cloth vertex entered a leg capsule\n")
		return
	var keys := _worst.keys()
	keys.sort_custom(func(a, b): return _worst[a] > _worst[b])
	for k in keys:
		print("   %-30s %6.1f mm  %4d verts  worst in %-10s" % [
			k, _worst[k] * 1000.0, _worst_n[k], _phase_of.get(k, "?")])
		print("        deepest vertex weights: %s" % _mix.get(k, "?"))
		var owner: String = _mix.get(k, "=").get_slice("=", 0)
		print("        on %s: contact seen %.1f mm, left after solving %.1f mm" % [
			owner, _saw.get(owner, 0.0) * 1000.0, _res.get(owner, 0.0) * 1000.0])
	print("   per movement state, worst cloth vertex inside a capsule:")
	for i in SWEEP.size():
		print("      %-12s %6.1f mm  %4d verts" % [
			"%s@%.0f" % [SWEEP[i][0], SWEEP[i][1]],
			_per_phase.get("%d" % i, 0.0) * 1000.0, _per_phase_n.get("%d" % i, 0)])
	print("")