extends SceneTree

## Do the five emotes move the character, differ from each other, and OVERLAP?
##
##     godot --path . -s res://debug/dance_check.gd
##
## Three properties, and the third is the one worth checking. "It moves" and
## "they are different" are easy to satisfy by accident — five sine waves at five
## frequencies would pass both and would still look like programmer animation.
## What separates a dance from an oscillation is that the body moves as a CHAIN:
## the hips lead and the head arrives later. That is measurable, so it is.
##
## Sampled from inside the modifier pass, like every other pose check here.
## Outside it Godot restores the local poses and what gets measured is the
## animation clip alone — every routine would report identical motion whether or
## not the dance layer exists at all.

const CAPTURE_BEATS := 4.0
const SAMPLES := 90

## A routine has to move the character at least this far, in metres of total
## head travel over the sample window. Below this it is not an emote.
const MIN_TRAVEL := 0.05
## Two routines must differ by at least this, comparing their per-frame pose
## trajectories.
const MIN_DISTINCT := 0.02

var _fails := 0
var _probe: Probe = null


class Probe extends SkeletonModifier3D:
	var pose: Array = []
	## Each bone's OWN local pose rotation, which is what a phase measurement
	## needs. A bone's GLOBAL rotation contains every ancestor's rotation too, so
	## the head's global carries the hips' un-lagged swing as a large component
	## and correlates with it at a lag of zero no matter how much the head itself
	## is delayed. Measuring globals reported Two-Step as having no overlap at
	## all when its head is delayed by five links.
	var local: Array = []

	func _process_modification() -> void:
		var skel := get_skeleton()
		if skel == null:
			return
		pose.resize(skel.get_bone_count())
		local.resize(skel.get_bone_count())
		for i in skel.get_bone_count():
			pose[i] = skel.get_bone_global_pose(i)
			local[i] = skel.get_bone_pose_rotation(i)


func _init() -> void:
	await process_frame

	var mgr = root.get_node_or_null("SkinManager")
	var skin = mgr.get_skin("taila") if mgr else null
	if skin == null or skin.model_path == "":
		print("dance_check: no rigged skin to test with")
		quit(1)
		return

	var model := SkinnedPlayerModel.new()
	root.add_child(model)
	model.skin_id = "taila"
	model.load_model(skin.model_path)
	for _i in 60:
		model.update_state("idle", 0.0, false)
		await process_frame

	if model._dance_mod == null:
		_expect(false, "the model built a dance layer")
		_done()
		return

	var skel: Skeleton3D = model.skeleton
	_probe = Probe.new()
	skel.add_child(_probe)
	skel.move_child(_probe, skel.get_child_count() - 1)

	_expect(DanceRoutines.count() == 5,
		"there are five emotes (%d)" % DanceRoutines.count())

	var tracks := {}
	for i in DanceRoutines.count():
		tracks[i] = await _sample(model, skel, i)

	_report(tracks)
	_compare(tracks)
	_done()


## One routine's trajectory: the hips' and head's positions, per frame, in the
## character's own space, plus the elbow angles for the joint-limit check.
func _sample(model, skel: Skeleton3D, index: int) -> Dictionary:
	model.set_dancing(true, index)
	# Let the blend arrive fully before recording, or the first routine sampled
	# reports smaller motion than the rest purely because it was still fading in.
	for _i in 40:
		model.update_state("idle", 0.0, false)
		await process_frame

	var mod = model._pose_mod
	var hips: int = mod._idx.get("DEF-hips", -1)
	var head: int = mod._idx.get("DEF-head", -1)
	# Overlap is measured between two links of the SAME chain, not between the
	# hips and the head.
	#
	# Every spine link is driven by the same channels (`spine_roll`, `spine_yaw`,
	# `spine_pitch`) at `beat - lag * i`, so the only difference between them IS
	# the lag. The hips and the head are driven by DIFFERENT channels, often at
	# different periods — Two-Step's hips roll on a two-beat cycle while its head
	# bobs on a one-beat one — so correlating those two compares signals that do
	# not have a phase relationship to find.
	var link_a: int = mod._idx.get("DEF-spine.001", -1)
	var link_b: int = mod._idx.get("DEF-spine.003", -1)
	var fa_r: int = mod._idx.get("DEF-forearm.R", -1)
	var ua_r: int = mod._idx.get("DEF-upper_arm.R", -1)
	var hand_r: int = mod._idx.get("DEF-hand.R", -1)

	var hip_track: Array = []
	var head_track: Array = []
	# The SAME quantity at two points in the chain: how far each bone has been
	# rotated away from its own rest pose, signed. Correlating the hips' world
	# TRANSLATION against the head's position RELATIVE to the hips was comparing
	# two different physical quantities driven by different channels at different
	# periods, and the peak landed anywhere — it reported the Robot, whose lag is
	# zero by construction, as the most overlapped routine in the set.
	var hip_rot: Array = []
	var head_rot: Array = []
	var worst_elbow := 180.0

	for _i in SAMPLES:
		model.update_state("idle", 0.0, false)
		await process_frame
		var pose: Array = _probe.pose
		if pose.size() <= maxi(hips, head) or hips < 0 or head < 0:
			continue
		var origin: Vector3 = pose[hips].origin
		hip_track.append(origin)
		head_track.append(pose[head].origin - origin)
		hip_rot.append(_twist(skel, _probe.local, link_a))
		head_rot.append(_twist(skel, _probe.local, link_b))
		# The elbow must never open past straight. A signed wave on a forearm
		# bends it backwards through the joint on half of every cycle, which is
		# the single most obvious tell in procedural animation.
		if ua_r >= 0 and fa_r >= 0 and hand_r >= 0 and pose.size() > hand_r:
			var upper: Vector3 = (pose[ua_r].origin - pose[fa_r].origin).normalized()
			var lower: Vector3 = (pose[hand_r].origin - pose[fa_r].origin).normalized()
			worst_elbow = minf(worst_elbow, rad_to_deg(acos(clampf(
				upper.dot(lower), -1.0, 1.0))))

	model.set_dancing(false)
	for _i in 30:
		model.update_state("idle", 0.0, false)
		await process_frame

	return {"hips": hip_track, "head": head_track, "elbow": worst_elbow,
		"hip_rot": hip_rot, "head_rot": head_rot}


func _report(tracks: Dictionary) -> void:
	print("\n=== EMOTES ===")
	for i in tracks:
		var t: Dictionary = tracks[i]
		var travel := _travel(t["head"])
		var hip_travel := _travel(t["hips"])
		var lag := _lag(t["hip_rot"], t["head_rot"])
		print("  %-12s head %.3f m   hips %.3f m   upper spine lags lower by %d frames   min elbow %.0f deg"
			% [DanceRoutines.name_of(i), travel, hip_travel, lag, t["elbow"]])


func _compare(tracks: Dictionary) -> void:
	for i in tracks:
		var t: Dictionary = tracks[i]
		var nm := DanceRoutines.name_of(i)
		_expect(_travel(t["head"]) >= MIN_TRAVEL,
			"%s actually moves the character (%.3f m)" % [nm, _travel(t["head"])])
		# 8 degrees of slack: the IK and the idle clip underneath both contribute,
		# and an elbow that never quite straightens is correct anyway.
		_expect(t["elbow"] >= 8.0,
			"%s never hyperextends the elbow (min %.0f deg)" % [nm, t["elbow"]])

	var ids: Array = tracks.keys()
	for i in ids.size():
		for j in range(i + 1, ids.size()):
			var d := _difference(tracks[ids[i]]["head"], tracks[ids[j]]["head"])
			_expect(d >= MIN_DISTINCT,
				"%s and %s are different dances (%.3f)"
					% [DanceRoutines.name_of(ids[i]), DanceRoutines.name_of(ids[j]), d])

	# OVERLAP. The head must trail the hips, because the body is a chain — this
	# is the property that separates a dance from five bones oscillating in
	# phase, and it is the whole reason `lag` exists in the routine data.
	#
	# The robot is exempt and deliberately so: its lag is zero on purpose, which
	# is what makes it read as mechanical against the other four.
	for i in tracks:
		var rid: String = String(DanceRoutines.get_routine(i).get("id", ""))
		# Robot: lag zero by construction, which is the point of it.
		# Spin: the head SPOTS — it holds its heading against the turn and whips
		# round to catch up, so it is deliberately not a delayed copy of the
		# hips. Asserting that it follows them would be asserting the opposite of
		# the technique.
		if rid == "robot" or rid == "spin":
			continue
		var lag := _lag(tracks[i]["hip_rot"], tracks[i]["head_rot"])
		_expect(lag > 0,
			"%s moves as a chain — the upper spine trails the lower by %d frames"
				% [DanceRoutines.name_of(i), lag])
	# The Robot's own property is that its motion is QUANTISED: it holds a pose
	# and jumps, where the others move continuously. That is what `steps` in the
	# routine data produces and what makes it read as mechanical against the
	# other four.
	#
	# Its LAG is deliberately not asserted. Zero lag ought to correlate perfectly
	# at shift 0, but the signal is a staircase with 16-frame plateaus, so many
	# shifts score nearly identically and the measured peak wanders — it reported
	# 21 frames. Asserting a number the measurement cannot resolve would be
	# asserting noise; the hold fraction below is the property that is actually
	# there.
	var robot := DanceRoutines.index_of("robot")
	var robot_step := _step_size(tracks[robot]["head_rot"])
	for i in tracks:
		if i == robot:
			continue
		var other := _step_size(tracks[i]["head_rot"])
		_expect(robot_step > other * 1.5,
			"Robot JUMPS between poses where %s flows (%.2f vs %.2f of range per frame)"
				% [DanceRoutines.name_of(i), robot_step, other])


## The largest single-frame change, as a fraction of the track's whole range.
##
## This is what quantised motion looks like from the outside: long flat stretches
## punctuated by one big jump. A smooth wave never moves more than a few percent
## of its range in a frame however punchy its easing.
##
## Measured as a JUMP rather than as time-spent-still, which was the first
## attempt and does not separate them: a shaped wave hangs at its extremes by
## design, so Two-Step scored the same 0.97 "holding" as the Robot did. The
## routines differ in HOW THEY LEAVE a pose, not in how long they sit in one.
func _step_size(rot_track: Array) -> float:
	var track := _project(rot_track)
	var n := track.size()
	if n < 4:
		return 0.0
	var lo := 1e30
	var hi := -1e30
	for v in track:
		lo = minf(lo, v)
		hi = maxf(hi, v)
	var span: float = hi - lo
	if span < 0.000001:
		return 0.0
	var biggest := 0.0
	for i in range(1, n):
		biggest = maxf(biggest, absf(track[i] - track[i - 1]))
	return biggest / span


## Total path length of a track.
func _travel(track: Array) -> float:
	var sum := 0.0
	for i in range(1, track.size()):
		sum += (track[i] as Vector3).distance_to(track[i - 1])
	return sum


## Mean per-frame distance between two tracks, after removing each one's own
## average position — otherwise two identical dances at different heights would
## read as different, and two different dances at the same height as the same.
func _difference(a: Array, b: Array) -> float:
	var n := mini(a.size(), b.size())
	if n == 0:
		return 0.0
	var ca := Vector3.ZERO
	var cb := Vector3.ZERO
	for i in n:
		ca += a[i]
		cb += b[i]
	ca /= float(n)
	cb /= float(n)
	var sum := 0.0
	for i in n:
		sum += ((a[i] - ca) - (b[i] - cb)).length()
	return sum / float(n)


## How many frames the head's rotation trails the hips', by NORMALISED
## cross-correlation.
##
## Both signals are the same quantity — a bone's rotation away from its own rest
## pose — sampled at two ends of the same chain, so the only thing that can
## differ between them is timing. That is the whole point: an unnormalised
## correlation between two DIFFERENT quantities peaks wherever their amplitudes
## happen to line up, which reported the Robot (lag zero by construction) as the
## most overlapped routine in the set.
##
## Pearson, so amplitude cannot influence where the peak falls — a head that
## moves further than the hips must not read as a head that moves later.
func _lag(hips: Array, head: Array) -> int:
	# BOTH ends projected onto the HIPS' axis, not each onto its own.
	#
	# Overlap is "the same motion, later", so the measurement has to be of the
	# same motion. Projecting each end onto its own dominant axis compares
	# whatever channel happens to dominate at that end, and routines drive
	# different channels at the two ends: Two-Step's hips are dominated by a
	# two-beat roll while its head is dominated by a one-beat bob, so the
	# correlation was between signals of different PERIOD and peaked wherever.
	var axis := _dominant_axis(hips)
	var a := _centre(_project(hips, axis))
	var b := _centre(_project(head, axis))
	var n := mini(a.size(), b.size())
	if n < 16:
		return 0
	var best := 0
	var best_score := -1e30
	# Out to half the window. The correlation of a periodic signal repeats every
	# period, so the search must stay inside one; Body Wave has the largest lag
	# in the set by design (0.13 s per link over five links, most of a beat at
	# 88 bpm) and a short window could not see it at all.
	for shift in range(0, n / 2):
		var sum := 0.0
		var na := 0.0
		var nb := 0.0
		for i in range(0, n - shift):
			sum += a[i] * b[i + shift]
			na += a[i] * a[i]
			nb += b[i + shift] * b[i + shift]
		if na < 0.000001 or nb < 0.000001:
			continue
		var score: float = sum / sqrt(na * nb)
		if score > best_score:
			best_score = score
			best = shift
	return best


## Mean-removed copy of a scalar track.
func _centre(track: Array) -> Array:
	var n := track.size()
	if n == 0:
		return []
	var mean := 0.0
	for v in track:
		mean += v
	mean /= float(n)
	var out: Array = []
	for v in track:
		out.append(v - mean)
	return out


## How far a bone has been rotated away from its rest pose, as a ROTATION VECTOR
## (axis times angle).
##
## A vector, not a signed scalar. The first version returned `angle * sign of the
## axis's largest component`, and that is discontinuous: as a rocking bone passes
## back through its rest pose the angle goes to zero and the axis FLIPS, so the
## signal jumped the full width of its range in a single frame. Two-Step measured
## a per-frame step of 0.99 of its own range — which looked exactly like the
## quantised motion the Robot is supposed to have exclusively, on a routine that
## is perfectly smooth.
##
## The rotation vector passes through zero and comes out the other side pointing
## the opposite way, which is continuous, and projecting it onto a fixed axis
## afterwards gives the signed wave the analysis actually wants.
func _twist(skel: Skeleton3D, local: Array, idx: int) -> Vector3:
	if idx < 0 or idx >= local.size():
		return Vector3.ZERO
	# The bone's OWN rotation away from its rest — not its global, which carries
	# every ancestor's along with it. See Probe.local.
	var rest: Quaternion = skel.get_bone_rest(idx).basis.get_rotation_quaternion()
	var d := (rest.inverse() * (local[idx] as Quaternion)).normalized()
	# Shortest arc, so a rotation just past 180 degrees does not read as one just
	# under -180.
	if d.w < 0.0:
		d = Quaternion(-d.x, -d.y, -d.z, -d.w)
	var ang := d.get_angle()
	if ang < 0.000001:
		return Vector3.ZERO
	return d.get_axis() * ang


## The axis a track of rotation vectors varies most about.
func _dominant_axis(track: Array) -> Vector3:
	var n := track.size()
	if n == 0:
		return Vector3.ZERO
	var mean := Vector3.ZERO
	for v in track:
		mean += v
	mean /= float(n)
	var axis := Vector3.ZERO
	var best := 0.0
	for v in track:
		var d: Vector3 = v - mean
		if d.length() > best:
			best = d.length()
			axis = d
	return axis.normalized() if axis.length() > 0.000001 else Vector3.ZERO


## A track of rotation vectors flattened to one signed scalar per frame, along
## `axis` — or along the track's own dominant axis if none is given.
func _project(track: Array, axis: Vector3 = Vector3.ZERO) -> Array:
	var n := track.size()
	if n == 0:
		return []
	var use := axis if axis.length() > 0.000001 else _dominant_axis(track)
	if use.length() < 0.000001:
		return []
	var mean := Vector3.ZERO
	for v in track:
		mean += v
	mean /= float(n)
	var out: Array = []
	for v in track:
		out.append((v - mean).dot(use))
	return out


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


func _done() -> void:
	print("\n=== DANCE SUMMARY ===")
	print("Failures: %d" % _fails)
	quit(1 if _fails > 0 else 0)