extends SkeletonModifier3D
class_name DanceModifier

## Drives a dance routine onto the skeleton, over whatever clip is playing.
##
## Runs AFTER the shooter pose layer and blends over it, so starting a dance
## takes the arms off the weapon smoothly rather than cutting, and stopping one
## hands them back the same way. The spring solver runs after both, so hair and
## cloth follow the dance without anything being asked to make that happen.
##
## The routine data — and the reasoning behind why the motion is built the way it
## is — lives in characters/dance_routines.gd. This is the machine that plays it.

## The routine to play. See DanceRoutines.ROUTINES.
var routine: Dictionary = {}
## 0..1. Eased by the owner, so a dance fades in and out rather than snapping.
var weight: float = 0.0
## `<model>.rig.json` roles, for bone resolution. See RigRoles.
var roles: Dictionary = {}

var _idx: Dictionary = {}
var _resolved := false
var _t: float = 0.0

## Per-link overlap, in seconds, from the hips outward. Index 0 is the hips.
const CHAIN := ["DEF-hips", "DEF-spine.001", "DEF-spine.002", "DEF-spine.003"]


func _process_modification() -> void:
	var skel := get_skeleton()
	if skel == null or weight <= 0.001 or routine.is_empty():
		return
	if not _resolved:
		_idx = RigRoles.resolve(skel, roles)
		_resolved = true

	var delta := get_physics_process_delta_time() if Engine.is_in_physics_frame() \
		else get_process_delta_time()
	_t += delta

	var bpm: float = float(routine.get("bpm", 100.0))
	var beat := _t * bpm / 60.0
	var lag: float = float(routine.get("lag", 0.05)) * bpm / 60.0
	var w := weight

	# ── Hips: the driver ────────────────────────────────────────────────────
	#
	# Translated, not just rotated. A body that never leaves its own axis reads
	# as a puppet on a stick; moving the hips is what makes the legs look like
	# they are carrying someone, and every other channel below is a reaction to
	# this one.
	var swing := _wave(beat, 2.0, 0.0)
	var bob := _wave(beat, float(routine.get("hip_bob_beats", 1.0)), 0.25)
	var hips: int = _idx.get("DEF-hips", -1)
	if hips >= 0:
		# Character-right is -X in skeleton space, up is +Y.
		var offset := Vector3(-swing * float(routine.get("hip_swing", 0.0)),
			bob * float(routine.get("hip_bob", 0.0)), 0.0) * w
		_offset_bone(skel, hips, offset)
		_add_space(skel, hips,
			Quaternion(Vector3(0, 0, 1), swing * float(routine.get("hip_roll", 0.0)) * w)
			* Quaternion(Vector3(0, 1, 0), swing * float(routine.get("hip_yaw", 0.0)) * w)
			* Quaternion(Vector3(1, 0, 0), bob * float(routine.get("hip_pitch", 0.0)) * w))

	# ── Spine: the same motion, later ───────────────────────────────────────
	#
	# Each link reads the wave at `beat - lag * i`, which is the whole of
	# overlapping action. It is one subtraction and it is the difference between
	# a rig oscillating and a person moving.
	var counter: float = float(routine.get("spine_counter", 0.0))
	var links := CHAIN.slice(1)
	var n := maxf(links.size(), 1)
	for i in links.size():
		var b: int = _idx.get(links[i], -1)
		if b < 0:
			continue
		var at := beat - lag * float(i + 1)
		var s := _wave(at, 2.0, 0.0)
		var v := _wave(at, float(routine.get("hip_bob_beats", 1.0)), 0.25)
		var q := Quaternion(Vector3(0, 0, 1),
				s * float(routine.get("spine_roll", 0.0)) * w / n) \
			* Quaternion(Vector3(0, 1, 0),
				(s * float(routine.get("spine_yaw", 0.0))
					- s * float(routine.get("hip_yaw", 0.0)) * counter) * w / n) \
			* Quaternion(Vector3(1, 0, 0),
				v * float(routine.get("spine_pitch", 0.0)) * w / n)
		_add_space(skel, b, q)

	_head(skel, beat, lag, w)
	_arms(skel, beat, lag, w)
	_legs(skel, beat, w)


## Head and neck: the last link in the chain, and the one carrying the spot.
func _head(skel: Skeleton3D, beat: float, lag: float, w: float) -> void:
	var at := beat - lag * float(CHAIN.size())
	var s := _wave(at, 2.0, 0.0)
	var v := _wave(at, float(routine.get("hip_bob_beats", 1.0)), 0.25)

	var yaw: float = s * float(routine.get("head_yaw", 0.0))

	# SPOTTING. A dancer turning keeps their head pointed at one place for as
	# long as they can, then whips it round to catch up. It is what stops them
	# getting dizzy, and it is the single most recognisable thing about a turn —
	# a head that simply rotates with the shoulders reads as a mannequin on a
	# turntable.
	#
	# So this is not a wave. The head COUNTERS the body's yaw exactly while the
	# hold lasts, then releases over a short window and lets the neck catch up.
	var spot: float = float(routine.get("spot", 0.0))
	if spot > 0.001:
		var body_yaw: float = s * float(routine.get("hip_yaw", 0.0)) \
			+ s * float(routine.get("spine_yaw", 0.0))
		# Where in the two-beat turn we are, 0..1.
		var u := fposmod(at / 2.0, 1.0)
		# Hold for the first 70%, then whip round over the next 20%, then arrive.
		var hold := 1.0 - smoothstep(0.70, 0.90, u)
		yaw -= body_yaw * spot * hold

	var q := Quaternion(Vector3(0, 1, 0), yaw * w) \
		* Quaternion(Vector3(0, 0, 1), s * float(routine.get("head_roll", 0.0)) * w) \
		* Quaternion(Vector3(1, 0, 0),
			(v * float(routine.get("head_bob", 0.0))
				+ v * float(routine.get("head_pitch", 0.0))) * w)
	# Split, so the whole column leans rather than the skull hinging off a rigid
	# neck.
	_add_space(skel, _idx.get("DEF-neck", -1), Quaternion.IDENTITY.slerp(q, 0.4))
	_add_space(skel, _idx.get("DEF-head", -1), Quaternion.IDENTITY.slerp(q, 0.6))


## Arms: out from the body, swinging in opposition, elbows folding on the beat.
##
## The two arms are half a cycle apart, which is what opposition is. Both read
## the wave later than the spine did, so the hands are the last thing to arrive —
## the end of the chain, where overlap is most visible.
func _arms(skel: Skeleton3D, beat: float, lag: float, w: float) -> void:
	var at := beat - lag * float(CHAIN.size() + 1)
	var beats: float = float(routine.get("arm_beats", 2.0))
	var out: float = float(routine.get("arm_out", 0.0))
	var swing: float = float(routine.get("arm_swing", 0.0))
	var elbow: float = float(routine.get("elbow", 0.0))

	for sign_i in 2:
		var right := sign_i == 0
		var side := 1.0 if right else -1.0
		var s := _wave(at + (0.0 if right else beats * 0.5), beats, 0.0)
		var ua: int = _idx.get("DEF-upper_arm." + ("R" if right else "L"), -1)
		var fa: int = _idx.get("DEF-forearm." + ("R" if right else "L"), -1)
		if ua >= 0:
			# Character-right is -X, so a positive Z rotation lifts the LEFT arm
			# and drops the right — hence the side flip. `out` is a static lift
			# that the swing then rides on top of, which is what stops the arms
			# from passing through the body at the bottom of the stroke.
			_add_space(skel, ua,
				Quaternion(Vector3(0, 0, 1), -side * (out + s * 0.35 * swing) * w)
				* Quaternion(Vector3(1, 0, 0), s * swing * w))
		if fa >= 0:
			# The elbow only ever folds, never hyperextends: a signed wave here
			# bends the forearm backwards through the joint on half of every
			# cycle, which is the most obvious possible tell.
			var fold := (0.5 + 0.5 * s) * elbow
			_add_space(skel, fa, Quaternion(Vector3(1, 0, 0), fold * w))


## Legs: knees absorbing the bob, out of phase with each other so the weight
## visibly transfers from one to the other.
func _legs(skel: Skeleton3D, beat: float, w: float) -> void:
	var knee: float = float(routine.get("knee", 0.0))
	if knee <= 0.001:
		return
	var beats: float = float(routine.get("hip_bob_beats", 1.0))
	for sign_i in 2:
		var right := sign_i == 0
		var s := _wave(beat + (0.0 if right else beats * 0.5), beats, 0.25)
		var thigh: int = _idx.get("DEF-thigh." + ("R" if right else "L"), -1)
		var shin: int = _idx.get("DEF-shin." + ("R" if right else "L"), -1)
		var bend := (0.5 + 0.5 * s) * knee
		# Thigh forward and shin back by twice as much, so the foot stays roughly
		# under the hip instead of the whole leg swinging out in front.
		_add_space(skel, thigh, Quaternion(Vector3(1, 0, 0), -bend * 0.5 * w))
		_add_space(skel, shin, Quaternion(Vector3(1, 0, 0), bend * w))


# ── The wave ─────────────────────────────────────────────────────────────────


## One channel's value at `beat`, in -1..1.
##
## `shape` bends a sine so it HANGS at the extremes and SNAPS between them, which
## is what an animator's key-and-breakdown pass produces and what a raw sine
## cannot. At shape 1 this is exactly `sin`; above it the curve gets punchier
## while staying continuous and staying in -1..1, so no amount of shaping can
## make a channel overshoot its authored amplitude.
##
## `steps` quantises the result instead, for the robot — the one routine whose
## whole point is that it does NOT move like the others.
func _wave(beat: float, beats: float, phase: float) -> float:
	if beats <= 0.001:
		return 0.0
	var u := beat / beats + phase
	var steps: int = int(routine.get("steps", 0))
	if steps > 0:
		# Hold a value for a whole step, then jump. Rounded rather than floored
		# so the extremes are actually reached — a floor never returns +1.
		return sin(TAU * (round(u * float(steps)) / float(steps)))
	var s := sin(TAU * u)
	var shape: float = float(routine.get("shape", 1.0))
	if is_equal_approx(shape, 1.0):
		return s
	return signf(s) * pow(absf(s), 1.0 / shape)


# ── Bone plumbing ────────────────────────────────────────────────────────────


## Move a bone by an offset expressed in SKELETON space.
##
## A bone's pose position is in its PARENT's space, so the offset has to be
## rotated out of skeleton space by the parent's rest basis first. Skipping that
## sends the hips sideways in whatever direction the rig happens to have called
## "x", which differs per character.
func _offset_bone(skel: Skeleton3D, idx: int, offset: Vector3) -> void:
	if idx < 0 or offset == Vector3.ZERO:
		return
	var parent := skel.get_bone_parent(idx)
	var local := offset
	if parent >= 0:
		local = skel.get_bone_global_rest(parent).basis.inverse() * offset
	skel.set_bone_pose_position(idx, skel.get_bone_rest(idx).origin + local)


## Compose a skeleton-space rotation onto a bone's animated local pose. Same
## contract as ShooterPoseModifier._add_space.
func _add_space(skel: Skeleton3D, idx: int, q_space: Quaternion) -> void:
	if idx < 0:
		return
	var b := skel.get_bone_global_rest(idx).basis.get_rotation_quaternion()
	var local := b.inverse() * q_space * b
	skel.set_bone_pose_rotation(idx, skel.get_bone_pose_rotation(idx) * local)