## LocomotionRig math (T-1088 design §4, §5, §10.4): per-leg constant-velocity
## derivation (cardinal 1.25 m/s / diagonal 1.77 m/s at Walk 400 ms), the 3x
## catch-up clamp, the 2.5 m snap threshold, idle hysteresis timing, paused-tick
## idempotence, blocked-move zero-motion (Q-020 bump-to-turn), and shortest-arc
## yaw easing under the per-stance TURN_BUDGET_DEG clamps.
##
## Pure static-core functions are tested directly; wrapper tests instantiate the
## rig out-of-tree (never added as a child, so the engine never drives _process —
## frames are stepped manually for deterministic timing).
class_name TestLocomotionMath
extends GdUnitTestSuite

const EPS := 0.000001

## 60 fps frame used by the yaw tests.
const DT := 1.0 / 60.0


func _make_rig() -> LocomotionRig:
	var rig: LocomotionRig = auto_free(LocomotionRig.new())
	rig.model_root = auto_free(Node3D.new())
	return rig


# -- leg-speed derivation (§4.1: dist / interval, clamped) --------------------------


func test_leg_speed_cardinal_walk() -> void:
	# One cardinal subtile at Walk: 0.5 m / 0.4 s = 1.25 m/s.
	assert_float(LocomotionRig.derive_leg_speed(0.5, 0.4)).is_equal_approx(1.25, EPS)


func test_leg_speed_diagonal_walk() -> void:
	# Diagonal step costs the same interval (no sqrt(2) on the wire, D-053):
	# 0.5 * sqrt(2) / 0.4 = ~1.77 m/s — arrives exactly when the next step lands.
	var diag := 0.5 * sqrt(2.0)
	assert_float(LocomotionRig.derive_leg_speed(diag, 0.4)).is_equal_approx(1.767767, 0.0001)


func test_leg_speed_sprint_cardinal() -> void:
	# Sprint window 200 ms: 0.5 / 0.2 = 2.5 m/s.
	assert_float(LocomotionRig.derive_leg_speed(0.5, 0.2)).is_equal_approx(2.5, EPS)


func test_leg_speed_catchup_clamped_at_3x() -> void:
	# 4-tile latest-wins delta at Walk: raw 2.0/0.4 = 5.0 m/s, clamped to
	# CATCHUP_MAX_FACTOR (3.0) * base = 3.75 m/s — feet speed up, never blur.
	assert_float(LocomotionRig.derive_leg_speed(2.0, 0.4)).is_equal_approx(3.75, EPS)


func test_leg_speed_floored_at_base() -> void:
	# Sub-subtile residue (late arrival) still closes at least at base speed —
	# the tail of a leg never crawls.
	assert_float(LocomotionRig.derive_leg_speed(0.1, 0.4)).is_equal_approx(1.25, EPS)


# -- target classification (§4.1: first snap / ignore / teleport / step) -------------


func test_classify_first_snapshot_snaps() -> void:
	var got := LocomotionRig.classify_target(
		false, Vector3.ZERO, Vector3.ZERO, Vector3(25.25, 0.0, 29.25)
	)
	assert_int(got).is_equal(LocomotionRig.TargetAction.SNAP_FIRST)


func test_classify_same_target_ignored() -> void:
	# Paused ticks keep delivering identical positions (§4.3) — idempotent.
	var target := Vector3(1.25, 0.0, 2.25)
	var got := LocomotionRig.classify_target(true, target, target, target)
	assert_int(got).is_equal(LocomotionRig.TargetAction.IGNORE)


func test_classify_same_target_ignored_mid_leg() -> void:
	# A repeat while still chasing keeps the current leg speed (no re-derivation
	# from the shrinking remaining distance).
	var target := Vector3(1.75, 0.0, 2.25)
	var render := Vector3(1.5, 0.0, 2.25)
	var got := LocomotionRig.classify_target(true, render, target, target)
	assert_int(got).is_equal(LocomotionRig.TargetAction.IGNORE)


func test_classify_step_within_snap_dist() -> void:
	var render := Vector3(1.25, 0.0, 2.25)
	var got := LocomotionRig.classify_target(
		true, render, render, render + Vector3(0.5, 0.0, 0.0)
	)
	assert_int(got).is_equal(LocomotionRig.TargetAction.STEP)


func test_classify_snap_threshold_boundary() -> void:
	# SNAP_DIST_M is strict: exactly 2.5 m (5 subtiles) still glides — matches
	# the 2D TELEPORT_DISTANCE_THRESHOLD semantics; beyond it teleports.
	var render := Vector3.ZERO
	var at_limit := LocomotionRig.classify_target(
		true, render, render, Vector3(2.5, 0.0, 0.0)
	)
	assert_int(at_limit).is_equal(LocomotionRig.TargetAction.STEP)
	var beyond := LocomotionRig.classify_target(
		true, render, render, Vector3(2.51, 0.0, 0.0)
	)
	assert_int(beyond).is_equal(LocomotionRig.TargetAction.TELEPORT)


func test_classify_teleport_measured_from_render_pos() -> void:
	# Teleport distance is render-pos -> new target (§4.1), not target -> target.
	var render := Vector3.ZERO
	var old_target := Vector3(2.0, 0.0, 0.0)
	var got := LocomotionRig.classify_target(
		true, render, old_target, Vector3(2.0, 0.0, 2.0)  # 2.83 m from render
	)
	assert_int(got).is_equal(LocomotionRig.TargetAction.TELEPORT)


# -- idle hysteresis (§4.2: IDLE_ENTER_DELAY_S = 0.18) --------------------------------


func test_moving_while_not_at_target() -> void:
	assert_bool(LocomotionRig.is_moving_state(false, 99.0)).is_true()


func test_hysteresis_holds_moving_within_delay() -> void:
	# Arrived, but only 0.1 s at target — still "moving" (covers 2-3 ticks of
	# snapshot jitter at every stance).
	assert_bool(LocomotionRig.is_moving_state(true, 0.1)).is_true()


func test_hysteresis_enters_idle_at_delay() -> void:
	assert_bool(LocomotionRig.is_moving_state(true, 0.18)).is_false()
	assert_bool(LocomotionRig.is_moving_state(true, 0.5)).is_false()


func test_idle_timer_accumulates_at_target() -> void:
	assert_float(LocomotionRig.advance_idle_timer(0.1, true, 0.05)).is_equal_approx(0.15, EPS)


func test_idle_timer_resets_when_leg_starts() -> void:
	assert_float(LocomotionRig.advance_idle_timer(0.5, false, 0.05)).is_equal_approx(0.0, EPS)


# -- yaw target selection (§5 authority table) -----------------------------------------


func test_facing_moving_uses_wire_octant() -> void:
	# Moving: server Facing IS the motion direction — mouse aim is visually ignored.
	var got := LocomotionRig.select_yaw_target(true, "East", "North", false, 0.0)
	assert_float(got).is_equal_approx(PI / 2.0, EPS)


func test_facing_moving_ignores_suppression() -> void:
	var got := LocomotionRig.select_yaw_target(true, "East", "North", true, 0.0)
	assert_float(got).is_equal_approx(PI / 2.0, EPS)


func test_facing_idle_uses_provider_octant() -> void:
	var got := LocomotionRig.select_yaw_target(false, "East", "North", false, 0.0)
	assert_float(got).is_equal_approx(PI, EPS)


func test_facing_idle_without_provider_falls_back_to_wire() -> void:
	# NPC path: no provider installed -> pure wire facing, single code path.
	var got := LocomotionRig.select_yaw_target(false, "East", "", false, 0.0)
	assert_float(got).is_equal_approx(PI / 2.0, EPS)


func test_facing_frozen_under_suppression_when_idle() -> void:
	# Dialogue / free camera: InputMapper stops sending octants — hold the last
	# target instead of showing an octant the server was never told.
	var held := 0.123
	var got := LocomotionRig.select_yaw_target(false, "East", "North", true, held)
	assert_float(got).is_equal_approx(held, EPS)


# -- yaw stepping (§5: shortest-arc lerp_angle under TURN_BUDGET_DEG) --------------------


func test_shortest_arc_wraps_through_pi() -> void:
	# -170 deg -> +170 deg is -20 deg through the seam, never +340 deg.
	var arc := LocomotionRig.shortest_arc(deg_to_rad(-170.0), deg_to_rad(170.0))
	assert_float(arc).is_equal_approx(deg_to_rad(-20.0), EPS)


func test_yaw_step_unclamped_matches_ease() -> void:
	# Small 5 deg correction at Walk: eased step (~1 deg at 60 fps) is far under
	# the 12 deg/frame budget — pure lerp_angle ease, no clamp.
	var weight := 1.0 - exp(-SandboxConstants.TURN_SHARPNESS * DT)
	var expected := weight * deg_to_rad(5.0)
	var got := LocomotionRig.step_yaw(0.0, deg_to_rad(5.0), "Walk", DT)
	assert_float(got).is_equal_approx(expected, EPS)


func test_yaw_budget_clamps_walk_reversal() -> void:
	# Near-180 reversal at Walk: eased step (~37 deg) hits the 720 deg/s budget
	# -> exactly 12 deg this frame (reversal completes in ~0.25 s, half a step).
	var got := LocomotionRig.step_yaw(0.0, deg_to_rad(179.0), "Walk", DT)
	assert_float(got).is_equal_approx(deg_to_rad(720.0) * DT, EPS)


func test_yaw_budget_per_stance() -> void:
	# Same reversal, different stances: Sprint 1080 -> 18 deg/frame,
	# Crouch 420 -> 7 deg/frame.
	var target := deg_to_rad(179.0)
	var sprint := LocomotionRig.step_yaw(0.0, target, "Sprint", DT)
	assert_float(sprint).is_equal_approx(deg_to_rad(1080.0) * DT, EPS)
	var crouch := LocomotionRig.step_yaw(0.0, target, "Crouch", DT)
	assert_float(crouch).is_equal_approx(deg_to_rad(420.0) * DT, EPS)


func test_yaw_budget_idle_and_unknown_key() -> void:
	# Idle budget 600 -> 10 deg/frame; unknown stance keys fall back to Idle.
	var target := deg_to_rad(179.0)
	var idle := LocomotionRig.step_yaw(0.0, target, "Idle", DT)
	assert_float(idle).is_equal_approx(deg_to_rad(600.0) * DT, EPS)
	var unknown := LocomotionRig.step_yaw(0.0, target, "Prone", DT)
	assert_float(unknown).is_equal_approx(deg_to_rad(600.0) * DT, EPS)


func test_yaw_step_takes_shortest_arc() -> void:
	# From -170 deg toward +170 deg: the step is negative (through the seam).
	var start := deg_to_rad(-170.0)
	var weight := 1.0 - exp(-SandboxConstants.TURN_SHARPNESS * DT)
	var expected := start + weight * deg_to_rad(-20.0)
	var got := LocomotionRig.step_yaw(start, deg_to_rad(170.0), "Walk", DT)
	assert_float(got).is_equal_approx(expected, EPS)


func test_yaw_step_wraps_result_across_seam() -> void:
	# Budget-clamped turn crossing -PI: -175 deg - 12 deg wraps to +173 deg.
	var got := LocomotionRig.step_yaw(deg_to_rad(-175.0), deg_to_rad(90.0), "Walk", DT)
	assert_float(got).is_equal_approx(deg_to_rad(173.0), EPS)


# -- wrapper behavior (out-of-tree rig, manually stepped frames) --------------------------


func test_first_wire_target_snaps_without_teleport_signal() -> void:
	var rig := _make_rig()
	var emissions: Array = []
	rig.teleported.connect(func(p: Vector3) -> void: emissions.append(p))
	rig.set_wire_target(Vector3(25.25, 0.0, 29.25), "East", "Walk", 100)
	assert_float(rig.position.x).is_equal_approx(25.25, EPS)
	assert_float(rig.position.z).is_equal_approx(29.25, EPS)
	assert_bool(rig.is_moving).is_false()
	assert_float(rig.current_speed).is_equal_approx(0.0, EPS)
	# Yaw snapped straight to the wire octant (East = +90 deg), no ease-in.
	assert_float(rig.model_root.rotation.y).is_equal_approx(PI / 2.0, EPS)
	assert_int(emissions.size()).is_equal(0)


func test_paused_repeat_of_same_target_is_idempotent() -> void:
	# Paused server keeps sending identical-position frames (§4.3): no motion
	# re-trigger, no drift, rig settles idle through normal hysteresis.
	var rig := _make_rig()
	var target := Vector3(1.25, 0.0, 2.25)
	rig.set_wire_target(target, "North", "Walk", 10)
	for i in 5:
		rig.set_wire_target(target, "North", "Walk", 10)
		rig._process(0.05)
	assert_bool(rig.is_moving).is_false()
	assert_float(rig.current_speed).is_equal_approx(0.0, EPS)
	assert_float(rig.velocity.length()).is_equal_approx(0.0, EPS)
	assert_float(rig.position.distance_to(target)).is_equal_approx(0.0, EPS)


func test_blocked_move_turns_in_place_without_motion() -> void:
	# Q-020 bump-to-turn (§4.3): the server updates Facing on a blocked move but
	# not position — the rig turns to face the wall and stands, zero motion.
	var rig := _make_rig()
	var target := Vector3(1.25, 0.0, 2.25)
	rig.set_wire_target(target, "North", "Walk", 1)  # snap: yaw = PI (North)
	rig.set_wire_target(target, "West", "Walk", 2)  # blocked: same tile, new facing
	rig._process(DT)
	assert_float(rig.position.distance_to(target)).is_equal_approx(0.0, EPS)
	assert_float(rig.current_speed).is_equal_approx(0.0, EPS)
	assert_bool(rig.is_moving).is_false()
	# Idle without a provider -> wire facing (West = -90 deg).
	assert_float(rig.yaw_target).is_equal_approx(-PI / 2.0, EPS)
	# One Idle-budget frame (600 deg/s -> 10 deg) from North toward West,
	# shortest arc through the +PI seam: PI + 10 deg wraps to -PI + 10 deg.
	assert_float(rig.model_root.rotation.y).is_equal_approx(-PI + deg_to_rad(10.0), EPS)


func test_step_moves_at_constant_leg_speed() -> void:
	# Equal displacement per frame — the M2 milestone's numeric core.
	var rig := _make_rig()
	var start := Vector3(1.25, 0.0, 2.25)
	rig.set_wire_target(start, "East", "Walk", 1)
	rig.set_wire_target(start + Vector3(0.5, 0.0, 0.0), "East", "Walk", 2)
	rig._process(0.1)
	assert_bool(rig.is_moving).is_true()
	assert_float(rig.current_speed).is_equal_approx(1.25, 0.001)
	assert_float(rig.velocity.x).is_equal_approx(1.25, 0.001)
	assert_float(rig.position.x).is_equal_approx(1.375, 0.001)
	rig._process(0.1)
	assert_float(rig.position.x).is_equal_approx(1.5, 0.001)


func test_diagonal_step_velocity_components() -> void:
	# Diagonal leg at Walk: 1.77 m/s along the diagonal = 1.25 m/s per axis —
	# both axes arrive exactly when a cardinal step would.
	var rig := _make_rig()
	var start := Vector3(1.25, 0.0, 2.25)
	rig.set_wire_target(start, "Southeast", "Walk", 1)
	rig.set_wire_target(start + Vector3(0.5, 0.0, 0.5), "Southeast", "Walk", 2)
	rig._process(0.1)
	assert_float(rig.current_speed).is_equal_approx(1.767767, 0.0001)
	assert_float(rig.velocity.x).is_equal_approx(1.25, 0.001)
	assert_float(rig.velocity.z).is_equal_approx(1.25, 0.001)


func test_arrival_holds_moving_through_hysteresis_window() -> void:
	# Arrive after 0.4 s, then stay "moving" until 0.18 s at target (§4.2).
	var rig := _make_rig()
	var start := Vector3(1.25, 0.0, 2.25)
	rig.set_wire_target(start, "East", "Walk", 1)
	rig.set_wire_target(start + Vector3(0.5, 0.0, 0.0), "East", "Walk", 2)
	for i in 4:  # 4 x 0.1 s = exactly one Walk window -> arrival
		rig._process(0.1)
	assert_float(rig.position.x).is_equal_approx(1.75, 0.001)
	assert_float(rig.current_speed).is_equal_approx(0.0, EPS)
	assert_bool(rig.is_moving).is_true()  # at target 0.1 s < 0.18 s — held
	rig._process(0.1)  # at target 0.2 s >= 0.18 s — idle
	assert_bool(rig.is_moving).is_false()


func test_teleport_snaps_all_channels_and_emits() -> void:
	var rig := _make_rig()
	var emissions: Array = []
	rig.teleported.connect(func(p: Vector3) -> void: emissions.append(p))
	rig.set_wire_target(Vector3(1.25, 0.0, 1.25), "North", "Walk", 1)
	var far := Vector3(25.25, 0.0, 29.25)  # Home-key Hub return — cross-map jump
	rig.set_wire_target(far, "South", "Walk", 2)
	assert_int(emissions.size()).is_equal(1)
	assert_float((emissions[0] as Vector3).x).is_equal_approx(far.x, EPS)
	assert_float(rig.position.distance_to(far)).is_equal_approx(0.0, EPS)
	assert_bool(rig.is_moving).is_false()
	assert_float(rig.current_speed).is_equal_approx(0.0, EPS)
	# Yaw hard-snapped to the post-teleport wire octant (South = 0), no glide.
	assert_float(rig.model_root.rotation.y).is_equal_approx(0.0, EPS)


func test_step_window_provider_overrides_default() -> void:
	# The player adapter injects InputMapper.MOVE_INTERVAL_MS — the rig itself
	# never reads the autoload (§4.0).
	var rig := _make_rig()
	rig.step_window_ms_provider = func(for_stance: String) -> float:
		return 200.0 if for_stance == "Sprint" else 400.0
	var start := Vector3(1.25, 0.0, 1.25)
	rig.set_wire_target(start, "East", "Sprint", 1)
	rig.set_wire_target(start + Vector3(0.5, 0.0, 0.0), "East", "Sprint", 2)
	rig._process(0.1)
	assert_float(rig.current_speed).is_equal_approx(2.5, 0.001)


func test_idle_facing_provider_steers_idle_yaw() -> void:
	# Idle aim: the provider's octant (client-local, already sent as SetFacing)
	# becomes the yaw target — mouse-responsive idle facing without wire lag.
	var rig := _make_rig()
	rig.idle_facing_provider = func() -> String: return "East"
	rig.set_wire_target(Vector3(1.25, 0.0, 2.25), "North", "Walk", 1)
	rig._process(DT)
	assert_bool(rig.is_moving).is_false()
	assert_float(rig.yaw_target).is_equal_approx(PI / 2.0, EPS)


func test_suppression_freezes_idle_yaw_target() -> void:
	# Dialogue / free camera (§5 row 3): the yaw target holds even though the
	# idle provider says otherwise.
	var rig := _make_rig()
	rig.idle_facing_provider = func() -> String: return "East"
	rig.suppression_provider = func() -> bool: return true
	rig.set_wire_target(Vector3(1.25, 0.0, 2.25), "North", "Walk", 1)
	rig._process(DT)
	assert_float(rig.yaw_target).is_equal_approx(PI, EPS)  # frozen at North
	# Node3D.rotation is float32: written +PI reads back a hair above double PI,
	# so wrap_yaw lands on the equivalent -PI. Assert angular identity, not sign.
	var arc := LocomotionRig.shortest_arc(PI, rig.model_root.rotation.y)
	assert_float(arc).is_equal_approx(0.0, EPS)