extends SceneTree

## Dev tool: WHY is the thigh inside the skirt?
##
##   godot --headless --path . -s res://debug/skirt_probe.gd
##
## debug/cloth_clip_check.gd says how deep the leg is inside the cloth;
## this says which part of the solver let it in. For every cloth bone it reports,
## at the worst moment of a run cycle:
##
##   TRUE   how far the deepest hull point is inside the REAL leg capsule
##   SEEN   how far the solver thinks it is inside — i.e. after the per-point
##          rest-clearance allowance in SpringBones._rest_clearances
##   t      where along the capsule that point sits (0 = hip joint, 1 = knee)
##
## TRUE >> SEEN means the allowance is the leak: the collision is satisfied while
## the leg is still visibly through the cloth. TRUE ~= SEEN means the solver sees
## the penetration and cannot correct it, which is a degree-of-freedom problem.
##
## Measured from an observer SkeletonModifier3D added AFTER SpringBones — see the
## rig-pipeline notes: _process runs before the modifier stack (rest pose) and
## force_update_all_bone_transforms() re-runs it (double-steps the solver).

const SWEEP := [["ground", 9.0], ["ground", 3.0], ["air", 6.0],
	["air", -8.0], ["slide", 10.0], ["dash", 14.0]]
const FRAMES_PER_STATE := 40

var _frames := 0
var _model: SkinnedPlayerModel = null
var _probe: Node = 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
	if _probe == null:
		var spring := skel.get_node_or_null("SpringBones")
		if spring == null:
			print("no SpringBones on this model — nothing to probe")
			return true
		_probe = Probe.new()
		_probe.name = "SkirtProbe"
		_probe.spring = spring
		skel.add_child(_probe)   # AFTER SpringBones, so it sees the final pose
		return false
	var phase: int = clampi((_frames - 9) / FRAMES_PER_STATE, 0, SWEEP.size() - 1)
	_model.update_state(SWEEP[phase][0], SWEEP[phase][1], false)
	_model.set_locomotion(0.0, 1.0, 0.0)
	if _frames > 9 + FRAMES_PER_STATE * SWEEP.size():
		_probe.effort = _model.skeleton.get_node("SpringBones").debug_effort_report()
		_probe.report()
		return true
	return false


## Reads the solver's own chain/collider tables and re-tests them against the
## final pose, so the numbers are the ones the solver actually acted on.
class Probe extends SkeletonModifier3D:
	var spring: Node = null
	var worst: Dictionary = {}   # bone name -> [true_pen, seen_pen, t, class]
	var effort: Dictionary = {}

	func _process_modification() -> void:
		var skel := get_skeleton()
		if skel == null or spring == null:
			return
		var to_world := skel.global_transform
		var cols: Array = spring._colliders
		for chain in spring._chains:
			var bones: PackedInt32Array = chain["bones"]
			var tips: PackedVector3Array = chain["tips"]
			var hulls: Array = chain["hulls"]
			var radii: Array = chain["radii"]
			if radii.is_empty():
				continue   # hair — does not collide with the legs by design
			for i in bones.size():
				var bone: int = bones[i]
				var posed: Transform3D = to_world * skel.get_bone_global_pose(bone)
				var pts := SpringBones._sample_points(
					posed, posed.origin, posed * tips[i], hulls[i])
				var rec: Dictionary = radii[i]
				var caps: PackedFloat32Array = rec.get("cap", PackedFloat32Array())
				var name := skel.get_bone_name(bone)
				for c in cols.size():
					var col: Dictionary = cols[c]
					# Same `from` offset the solver uses, or this reports overlap with
					# a part of the leg nothing is being asked to clear.
					var ends: Array = spring._capsule(skel, to_world, col)
					var a: Vector3 = ends[0]
					var b: Vector3 = ends[1]
					var ab := b - a
					var d2 := ab.length_squared()
					for j in pts.size():
						var p: Vector3 = pts[j]
						var t := 0.0 if d2 < 0.000001 \
							else clampf((p - a).dot(ab) / d2, 0.0, 1.0)
						var dist := p.distance_to(a + ab * t)
						var r_true: float = lerpf(float(col["rh"]), float(col["rt"]), t)
						var idx := c * pts.size() + j
						var r_seen: float = minf(r_true,
							caps[idx] if idx < caps.size() else r_true)
						var pen := r_true - dist
						if pen > float(worst.get(name, [0.0])[0]):
							# `reach` is the lever the solver has on this point:
							# a collision is resolved by ROTATING the bone, so a
							# point sitting almost on the bone's own head cannot
							# be moved by it at all, however deep it is.
							# `above` is its height over the hip joint — anything
							# positive is inside the fictional sphere the capsule
							# puts at the top of the thigh, not inside the leg.
							worst[name] = [pen, r_seen - dist, t,
								(p - posed.origin).length(), p.y - a.y]

	func report() -> void:
		var skel := get_skeleton()
		print("\n=== deepest leg-in-cloth per bone (worst over the sweep) ===")
		print("   %-26s %8s %8s %6s %8s %9s %8s %8s" % [
			"bone", "TRUE mm", "SEEN mm", "t", "reach mm", "above mm", "fix deg", "left mm"])
		var keys := worst.keys()
		keys.sort_custom(func(a, b): return worst[a][0] > worst[b][0])
		for k in keys:
			var w: Array = worst[k]
			if w[0] <= 0.0005:
				continue
			var ef: Array = effort.get(skel.find_bone(k), [0.0, 0.0])
			print("   %-26s %8.1f %8.1f %6.2f %8.1f %9.1f %8.1f %8.1f" % [
				k, w[0] * 1000.0, w[1] * 1000.0, w[2], w[3] * 1000.0, w[4] * 1000.0,
				ef[0], ef[1]])
		print("")