settled-reach/client/scripts/sandbox/locomotion_rig.gd

class_name LocomotionRig
extends Node3D
## T-1088 locomotion rig (design §4, §5) — the position and yaw channels of the
## feel thesis (§0): each channel gets exactly ONE smoothing layer, crisp -> soft.
## Thin Node3D wrapper (this node = PlayerRig; local position = interpolated sim
## position in WorldRoot-local metres) over a pure static math core, so every
## rule is headless-testable (test_locomotion_math.gd, §10.4). Proto-production:
## an NPC adapter later feeds set_wire_target() from VisibleEntity rows with no
## rig changes.
##
## Position (§4.1): per-leg constant velocity sized to the step — on a changed
## target, cover the distance in exactly one stance interval (dist/interval),
## clamped to [base, CATCHUP_MAX_FACTOR * base]. No easing on the character
## itself: easing a repeating 0.4 s step reads as scooting; the camera (S5) is
## deliberately the only soft position layer.
##
## Yaw (§5, "server feet, client eyes"): moving (incl. the idle-hysteresis
## window) follows the wire octant — the server sets Facing from the move delta,
## so it IS the motion direction; idle follows idle_facing_provider (the same
## snapped octant that rides the SetFacing wire); frozen while suppressed
## (dialogue / free camera). Shortest-arc lerp_angle ease (TURN_SHARPNESS) under
## per-stance deg/s budgets (TURN_BUDGET_DEG), written to ModelRoot.rotation.y in
## WorldRoot-LOCAL space so octant->yaw composes with the D-148 map rotation
## exactly once.
##
## TRAP (design §2, §5): NEVER call CharacterVisual.set_facing() from this rig
## or anything near it. Its internal table (west=+90, east=-90 —
## character_visual.gd:184-193) assumes a mirrored axis mapping (East -> -X)
## incompatible with THE convention (SandboxSpace: East -> +X); using it makes
## the model face west while walking east. This rig owns ModelRoot.rotation.y
## exclusively, always through SandboxSpace.octant_to_yaw(); CharacterVisual's
## own rotation.y stays 0.
##
## The rig never reads GameState, InputMapper, or any autoload itself (§4.0) —
## the sandbox root adapts snapshot fields into set_wire_target() and installs
## the provider Callables below. NPC adapters leave the providers unset.

## Emitted when a wire target lands beyond SNAP_DIST_M (Home-key / cross-map
## teleport, §4.1): all rig channels have already snapped; the camera (S5)
## hard-sets its pivot on this, the anim machine (S6) hard-cuts (BLEND.teleport
## = 0.0). NOT emitted for the first-ever snapshot (the root's latch owns that).
signal teleported(pos_m: Vector3)

## set_wire_target() outcome — a pure decision, exposed for tests (§10.4).
enum TargetAction { SNAP_FIRST, IGNORE, TELEPORT, STEP }

## Fallback step window when step_window_ms_provider is unset (headless tests,
## future NPC adapters before MovementSpeed wiring). Mirrors the Walk default
## stance only — the player adapter MUST install a provider reading
## InputMapper.MOVE_INTERVAL_MS at runtime: that table is the single source and
## is deliberately never copied (design §13.8).
const DEFAULT_STEP_WINDOW_MS := 400.0

## (stance: String) -> float|int step window in ms for one confirmed step.
## Player adapter: func(stance): return InputMapper.MOVE_INTERVAL_MS.get(stance, 400).
## Called once per accepted step (per-leg recompute, §4.1) with the stance that
## arrived in the same wire target.
var step_window_ms_provider: Callable = Callable()
## () -> octant String for idle facing. The player adapter returns
## InputMapper.facing_octant — the SAME snapped octant that rides the SetFacing
## wire, so the model never shows an octant the server wasn't told (§5). Leave
## unset (NPCs) to collapse the facing split to pure wire facing — single code
## path by construction.
var idle_facing_provider: Callable = Callable()
## T-1088 follow-facing: while a client-initiated path-follow is active the
## body yaw commits to the leg direction (mouse -> head_look, not body).
## Set/cleared by the sandbox root from PathFollower.is_active().
var commit_body_to_motion: bool = false
## () -> bool, true while input is suppressed. The player adapter returns
## GameState.dialogue_active or GameState.free_camera_mode — the exact condition
## under which InputMapper computes but does not send octants (input_mapper.gd:71).
## While idle + suppressed the yaw target freezes (last target held, §5). Leave
## unset for never-suppressed (NPCs).
var suppression_provider: Callable = Callable()

## Latest wire facing octant (snapshot player_facing) — the moving yaw source.
## Updates on EVERY wire target, including ignored ones: a blocked move (Q-020)
## still changes Facing server-side, so bump-to-turn falls out for free (§4.3).
var wire_octant: String = "North"
## Latest wire stance — selects the step window and the moving turn budget.
var stance: String = "Walk"
## Tick of the latest consumed wire target (DebugHud "snapshot age" input).
var last_wire_tick: int = -1

## True until at-target for IDLE_ENTER_DELAY_S (§4.2 hysteresis — kills the
## walk<->idle flicker from snapshot jitter). Gait selection keys off this and
## current_speed, never off input (§0).
var is_moving: bool = false
## Constant per-leg ground speed (m/s), zero on arrival — cadence-sync input
## (§6.2: speed_scale = current_speed / NATIVE_MPS, constant per leg).
var current_speed: float = 0.0
## Render velocity (m/s), zero on arrival — gait-selection input (§0).
var velocity: Vector3 = Vector3.ZERO
## Current yaw target (rad, WorldRoot-local) — held while frozen. DebugHud pairs
## it with model_root.rotation.y as "yaw target / actual".
var yaw_target: float = 0.0

## Yaw channel output node — resolved from the ModelRoot scene child in _ready();
## injectable beforehand for headless tests.
var model_root: Node3D = null

var _has_target: bool = false
var _target_pos: Vector3 = Vector3.ZERO
var _leg_speed: float = 0.0
var _idle_timer_s: float = 0.0


func _ready() -> void:
	if model_root == null:
		model_root = get_node_or_null("ModelRoot") as Node3D


## The single rig input (§4.0): the sandbox root (or a future NPC adapter) calls
## this once per consumed snapshot with the wire position converted to
## WorldRoot-local metres (SandboxSpace.wire_to_world), the wire facing octant,
## stance, and tick.
func set_wire_target(pos_m: Vector3, facing_octant: String, new_stance: String, tick: int) -> void:
	wire_octant = facing_octant
	stance = new_stance
	last_wire_tick = tick
	match classify_target(_has_target, position, _target_pos, pos_m):
		TargetAction.SNAP_FIRST:
			# First-ever snapshot snaps, no signal (2D precedent
			# entity_renderer.gd:133-140; the root's first-snapshot latch
			# handles the camera).
			_snap_all_channels(pos_m)
		TargetAction.IGNORE:
			# Paused-tick identical frames and blocked moves (Q-020) land here:
			# target-chasing is idempotent, nothing re-triggers. Facing already
			# updated above — bump-to-turn is the visible blocked-move behavior.
			pass
		TargetAction.TELEPORT:
			_snap_all_channels(pos_m)
			teleported.emit(pos_m)
		TargetAction.STEP:
			_target_pos = pos_m
			_leg_speed = derive_leg_speed(position.distance_to(pos_m), _step_interval_s())


func _process(delta: float) -> void:
	if not _has_target:
		return
	# Position channel: constant-velocity chase, exact arrival (§4.1).
	position = step_position(position, _target_pos, _leg_speed, delta)
	var at_target := position.is_equal_approx(_target_pos)
	if at_target:
		position = _target_pos  # kill sub-epsilon residue
		velocity = Vector3.ZERO
		current_speed = 0.0
	else:
		velocity = (_target_pos - position).normalized() * _leg_speed
		current_speed = _leg_speed
	_idle_timer_s = advance_idle_timer(_idle_timer_s, at_target, delta)
	is_moving = is_moving_state(at_target, _idle_timer_s)
	# Yaw channel (§5).
	_update_facing(delta)


## Remaining distance (m) on the current leg — DebugHud "leg distance" readout.
func leg_remaining_m() -> float:
	return position.distance_to(_target_pos)


## DebugHud duck contract (sandbox_debug_hud.gd): render speed in m/s — constant
## per leg, zero on arrival.
func get_render_speed() -> float:
	return current_speed


## DebugHud duck contract (sandbox_debug_hud.gd): current yaw target (rad,
## WorldRoot-local), paired on screen with ModelRoot.rotation.y as "target/actual".
func get_yaw_target_rad() -> float:
	return yaw_target


## Snap every rig channel to the target (first snapshot / teleport, §4.1):
## position, velocity, yaw target AND actual (no ease across a jump), and the
## idle hysteresis (pre-expired so the rig is instantly idle).
func _snap_all_channels(pos_m: Vector3) -> void:
	_has_target = true
	position = pos_m
	_target_pos = pos_m
	_leg_speed = 0.0
	velocity = Vector3.ZERO
	current_speed = 0.0
	_idle_timer_s = SandboxConstants.IDLE_ENTER_DELAY_S
	is_moving = false
	yaw_target = SandboxSpace.octant_to_yaw(wire_octant)
	if model_root != null:
		model_root.rotation.y = yaw_target


func _step_interval_s() -> float:
	var window_ms := DEFAULT_STEP_WINDOW_MS
	if step_window_ms_provider.is_valid():
		window_ms = float(step_window_ms_provider.call(stance))
	return window_ms / 1000.0


func _update_facing(delta: float) -> void:
	var suppressed := false
	if suppression_provider.is_valid():
		suppressed = bool(suppression_provider.call())
	if commit_body_to_motion and is_moving and velocity.length_squared() > 0.0001:
		# Path-follow (T-1088 follow-facing refinement): the body is committed to
		# the leg direction — the mouse drives the head/torso look-at instead
		# (head_look.gd), and SetFacing still steers the server vision cone.
		# Leg direction in WorldRoot-local space -> yaw via the same convention
		# as SandboxSpace (local +X = sim East, +Z = sim South).
		yaw_target = SandboxSpace.sim_angle_to_yaw(atan2(velocity.z, velocity.x))
	else:
		var idle_octant := ""
		if idle_facing_provider.is_valid():
			idle_octant = str(idle_facing_provider.call())
		yaw_target = select_yaw_target(is_moving, wire_octant, idle_octant, suppressed, yaw_target)
	if model_root == null:
		return
	var budget_key := stance if is_moving else "Idle"
	model_root.rotation.y = step_yaw(model_root.rotation.y, yaw_target, budget_key, delta)


# ---------------------------------------------------------------------------
# Pure static core — all locomotion math lives below, stateless and
# headless-testable (§10.4). The wrapper above only shuttles state.
# ---------------------------------------------------------------------------


## Classify an incoming wire target (§4.1): the first-ever snapshot snaps; an
## unchanged target is ignored — paused-tick frames (§4.3) and blocked moves
## (Q-020) re-trigger nothing by construction, and a mid-leg repeat keeps the
## current leg speed (no tail-slowdown re-derivation); beyond SNAP_DIST_M
## (5 subtiles — matches the 2D TELEPORT_DISTANCE_THRESHOLD) is a teleport;
## otherwise a normal step.
static func classify_target(
	has_target: bool, render_pos: Vector3, current_target: Vector3, new_target: Vector3
) -> TargetAction:
	if not has_target:
		return TargetAction.SNAP_FIRST
	if new_target.is_equal_approx(current_target):
		return TargetAction.IGNORE
	if render_pos.distance_to(new_target) > SandboxConstants.SNAP_DIST_M:
		return TargetAction.TELEPORT
	return TargetAction.STEP


## Per-leg constant velocity (§4.1): cover dist in exactly one stance interval,
## clamped to [base, CATCHUP_MAX_FACTOR * base] where base is the cardinal
## one-subtile speed (SUBTILE_M / interval). One rule handles everything:
## cardinal Walk 0.5/0.4 = 1.25 m/s; diagonal 0.707/0.4 = 1.77 m/s — arrives
## exactly on time (no sqrt(2) on the wire, D-053; a fixed speed would lag on
## held diagonals); multi-tile latest-wins deltas close within ~one interval
## with feet speeding up to match (§6.2).
static func derive_leg_speed(dist_m: float, interval_s: float) -> float:
	var safe_interval := maxf(interval_s, 0.001)
	var base := SandboxConstants.SUBTILE_M / safe_interval
	return clampf(dist_m / safe_interval, base, SandboxConstants.CATCHUP_MAX_FACTOR * base)


## One frame of constant-velocity chase — move_toward clamps at the target, so
## arrival is exact and velocity is a clean square wave, never a sawtooth (§4.1).
static func step_position(
	render_pos: Vector3, target: Vector3, leg_speed: float, delta: float
) -> Vector3:
	return render_pos.move_toward(target, leg_speed * delta)


## Idle-hysteresis timer (§4.2): accumulates while at-target, resets the moment
## a new leg starts.
static func advance_idle_timer(timer_s: float, at_target: bool, delta: float) -> float:
	return timer_s + delta if at_target else 0.0


## is_moving with hysteresis (§4.2): snapshot arrival jitters +-1-2 ticks against
## the client throttle, so the rig frequently arrives a few frames early —
## staying "moving" until at-target for IDLE_ENTER_DELAY_S kills the walk<->idle
## flicker; a real stop still reaches Idle inside the human ~0.3 s settle window.
static func is_moving_state(at_target: bool, timer_s: float) -> bool:
	if not at_target:
		return true
	return timer_s < SandboxConstants.IDLE_ENTER_DELAY_S


## Yaw target source per the §5 authority table ("server feet, client eyes"):
## moving -> wire octant (server Facing IS the motion direction); idle +
## suppressed -> held target frozen; idle with a provider -> the client-local
## aim octant; idle without one (NPCs) -> pure wire facing. Empty idle_octant
## means "no provider installed".
static func select_yaw_target(
	moving: bool, wire_oct: String, idle_octant: String, suppressed: bool, held_target: float
) -> float:
	if moving:
		return SandboxSpace.octant_to_yaw(wire_oct)
	if suppressed:
		return held_target
	if idle_octant.is_empty():
		return SandboxSpace.octant_to_yaw(wire_oct)
	return SandboxSpace.octant_to_yaw(idle_octant)


## Signed shortest arc from one yaw to another, in [-PI, PI).
static func shortest_arc(from_yaw: float, to_yaw: float) -> float:
	return wrapf(to_yaw - from_yaw, -PI, PI)


## Wrap a yaw into (-PI, PI] — same convention as SandboxSpace (North stays +PI).
static func wrap_yaw(yaw: float) -> float:
	var wrapped := yaw
	while wrapped > PI:
		wrapped -= TAU
	while wrapped <= -PI:
		wrapped += TAU
	return wrapped


## One frame of yaw smoothing (§5): shortest-arc lerp_angle ease-out at
## TURN_SHARPNESS, with the per-frame change clamped to the per-stance
## TURN_BUDGET_DEG (budget_key = stance while moving, "Idle" otherwise; unknown
## keys fall back to Idle). A 180 deg reversal at Walk completes in ~0.25 s
## (about half a step); a 45 deg corner resolves in ~60 ms.
static func step_yaw(current_yaw: float, target_yaw: float, budget_key: String, delta: float) -> float:
	var eased := lerp_angle(current_yaw, target_yaw, 1.0 - exp(-SandboxConstants.TURN_SHARPNESS * delta))
	var budget_deg: float = SandboxConstants.TURN_BUDGET_DEG.get(
		budget_key, SandboxConstants.TURN_BUDGET_DEG["Idle"]
	)
	var max_step := deg_to_rad(budget_deg) * delta
	var step := clampf(shortest_arc(current_yaw, eased), -max_step, max_step)
	return wrap_yaw(current_yaw + step)