extends RefCounted

## NEO-22 WASD wall scrape / floor seam tick (extracted from [code]player.gd[/code] for
## [kbd]gdlint[/kbd] [code]max-file-lines[/code]). Owns coyote + scrape feet state; calls back into
## the [CharacterBody3D] player for physics and walk helpers.

const LOCOMOTION_FLOOR_COYOTE_TICKS: int = 8
const LOCOMOTION_WALL_SCRAPE_PROBE_HOLD_VY_MAX: float = -1.05
const LOCOMOTION_WALL_SCRAPE_SNAP_FALLBACK: float = 0.12
const LOCOMOTION_SCRAPE_FEET_LOCK_LERP: float = 0.2
const LOCOMOTION_SCRAPE_FEET_CORRECT_MAX: float = 0.008
const LOCOMOTION_SCRAPE_FEET_ERR_GATE: float = 0.075
const LOCOMOTION_SCRAPE_RAY_FEET_ABSORB_MAX: float = 0.1
const LOCOMOTION_SCRAPE_RAY_PROBE_Y_LERP: float = 0.42
const LOCOMOTION_SCRAPE_RAY_FEET_MEDIAN_SPAN: float = 0.055
const LOCOMOTION_SCRAPE_WISH_LINE_MICRO_PERP_M: float = 0.032
## Sentinel when no walkable floor hit (tests compare to this).
const FLOOR_RAY_FEET_INVALID: float = -1.0e9
## Must match [code]player.gd[/code] walk continuation probe (same ray math as
## [method _walk_ray_hit_up_floor]).
const _WALK_SUPPORT_PROBE_DEPTH: float = 0.32
const _WALK_SUPPORT_PROBE_MIN_UP_DOT: float = 0.42
const _WALK_CONTINUATION_MAX_BELOW_FEET: float = 0.108

var _player: CharacterBody3D
var _cap_half: float
var _cap_radius: float
var _move_speed: float
var _floor_snap_moving: float

var _floor_coyote: int = 0
var _scrape_feet_lock_valid: bool = false
var _scrape_feet_y_lock: float = 0.0
var _scrape_probe_y_ema_valid: bool = false
var _scrape_probe_y_ema: float = 0.0


## Pure: walk continuation rules after a downward feet probe hit (same thresholds as
## [method _floor_ray_feet_y]).
static func evaluate_floor_ray_hit_y(
	feet_y: float,
	hit_y: float,
	normal: Vector3,
	continuation_max_below_feet: float,
	min_floor_up_dot: float
) -> float:
	if hit_y < feet_y - continuation_max_below_feet:
		return FLOOR_RAY_FEET_INVALID
	if normal.dot(Vector3.UP) <= min_floor_up_dot:
		return FLOOR_RAY_FEET_INVALID
	return hit_y


## Pure: median of one or more feet Y samples (sorted); empty → [constant FLOOR_RAY_FEET_INVALID].
static func median_feet_y_from_samples(samples: Array[float]) -> float:
	if samples.is_empty():
		return FLOOR_RAY_FEET_INVALID
	var y_vals: Array[float] = samples.duplicate()
	y_vals.sort()
	var mid: int = (y_vals.size() - 1) >> 1
	return y_vals[mid]


## Pure: XZ after micro-slip projection onto the wish line (or unchanged if out of band).
static func xz_after_micro_slip_projection(
	anchor_xz: Vector2, player_xz: Vector2, wish_xz: Vector2, perp_limit_m: float
) -> Vector2:
	var w2s: float = wish_xz.length_squared()
	if w2s < 1e-12:
		return player_xz
	var w2n: Vector2 = wish_xz / sqrt(w2s)
	var along: float = (player_xz - anchor_xz).dot(w2n)
	var p2_on: Vector2 = anchor_xz + w2n * along
	var perp_len: float = (player_xz - p2_on).length()
	if perp_len > perp_limit_m or perp_len <= 1e-7:
		return player_xz
	return Vector2(p2_on.x, p2_on.y)


## Pure: non-negative wish-aligned horizontal speed, capped at [param move_speed].
static func horizontal_velocity_aligned_to_wish(
	wish_xz: Vector2, vel_xz: Vector2, move_speed: float
) -> Vector2:
	if wish_xz.length_squared() < 1e-10:
		return vel_xz
	var w2: Vector2 = wish_xz.normalized()
	var along: float = vel_xz.dot(w2)
	if along < 0.0:
		along = 0.0
	along = minf(move_speed, along)
	return Vector2(w2.x * along, w2.y * along)


func _init(
	player: CharacterBody3D,
	cap_half: float,
	cap_radius: float,
	move_speed: float,
	floor_snap_moving: float
) -> void:
	_player = player
	_cap_half = cap_half
	_cap_radius = cap_radius
	_move_speed = move_speed
	_floor_snap_moving = floor_snap_moving


func reset_scrape_feet_lock() -> void:
	_scrape_feet_lock_valid = false
	_scrape_probe_y_ema_valid = false


func reset_for_nav_or_snap() -> void:
	_floor_coyote = 0
	reset_scrape_feet_lock()


func _feet_y() -> float:
	return _player.global_position.y - _cap_half - _cap_radius


func _has_scrape_vertical_contact() -> bool:
	if _player.is_on_wall():
		return true
	for i: int in _player.get_slide_collision_count():
		var n: Vector3 = _player.get_slide_collision(i).get_normal()
		if n.y < 0.48 and n.y > -0.22:
			return true
	return false


func _project_global_xz_on_wish_line_if_micro_slip(anchor_xz: Vector2, wish_xz: Vector2) -> void:
	var p2: Vector2 = Vector2(_player.global_position.x, _player.global_position.z)
	var new_xz: Vector2 = xz_after_micro_slip_projection(
		anchor_xz, p2, wish_xz, LOCOMOTION_SCRAPE_WISH_LINE_MICRO_PERP_M
	)
	if new_xz.is_equal_approx(p2):
		return
	_player.global_position.x = new_xz.x
	_player.global_position.z = new_xz.y


func _align_velocity_xz_to_wish() -> void:
	var wish: Vector3 = _player._locomotion_wish_world_xz
	var w2 := Vector2(wish.x, wish.z)
	var v2 := Vector2(_player.velocity.x, _player.velocity.z)
	var out: Vector2 = horizontal_velocity_aligned_to_wish(w2, v2, _move_speed)
	_player.velocity.x = out.x
	_player.velocity.z = out.y


func _floor_ray_feet_y(
	space: PhysicsDirectSpaceState3D, anchor_feet_xz: Vector2, off_xz: Vector2
) -> float:
	var feet_y: float = _player.global_position.y - _cap_half - _cap_radius
	var feet_x: float = anchor_feet_xz.x + off_xz.x
	var feet_z: float = anchor_feet_xz.y + off_xz.y
	var probe_from := Vector3(feet_x, feet_y + 0.04, feet_z)
	var probe_end := Vector3(feet_x, feet_y - _WALK_SUPPORT_PROBE_DEPTH, feet_z)
	var q := PhysicsRayQueryParameters3D.create(probe_from, probe_end)
	q.collision_mask = _player.collision_mask
	q.exclude = [_player.get_rid()]
	var res: Dictionary = space.intersect_ray(q)
	if res.is_empty() or not res.has("normal") or not res.has("position"):
		return FLOOR_RAY_FEET_INVALID
	var hit_y: float = (res["position"] as Vector3).y
	var n: Vector3 = res["normal"]
	return evaluate_floor_ray_hit_y(
		feet_y, hit_y, n, _WALK_CONTINUATION_MAX_BELOW_FEET, _WALK_SUPPORT_PROBE_MIN_UP_DOT
	)


func _floor_ray_feet_y_median(
	space: PhysicsDirectSpaceState3D, anchor_feet_xz: Vector2, wish_probe: Vector2
) -> float:
	var offs: Array[Vector2] = [Vector2.ZERO]
	var ws2: float = wish_probe.length_squared()
	if ws2 > 1e-10:
		var wp: Vector2 = wish_probe / sqrt(ws2)
		var perp: Vector2 = Vector2(-wp.y, wp.x) * LOCOMOTION_SCRAPE_RAY_FEET_MEDIAN_SPAN
		offs.append(perp)
		offs.append(-perp)
	var y_vals: Array[float] = []
	for k: int in range(offs.size()):
		var y_one: float = _floor_ray_feet_y(space, anchor_feet_xz, offs[k])
		if y_one != FLOOR_RAY_FEET_INVALID:
			y_vals.append(y_one)
	return median_feet_y_from_samples(y_vals)


func run_wasd(delta: float) -> void:
	if _player._has_walk_goal:
		_player.clear_nav_goal()
	_player._idle_anchor_active = false
	_player.floor_block_on_wall = true
	var scrape_ray_anchor_feet_xz := Vector2(_player.global_position.x, _player.global_position.z)
	var feet_y: float = _feet_y()
	var wish: Vector3 = _player._locomotion_wish_world_xz
	_player.velocity.x = wish.x * _move_speed
	_player.velocity.z = wish.z * _move_speed
	var wish_probe: Vector2 = Vector2(wish.x, wish.z)
	var wish_probe_active: bool = wish_probe.length_squared() > 1e-10
	var locomotion_probe_floor: bool = (
		wish_probe_active and _player._walk_has_close_floor_probe_below(wish_probe)
	)
	# Slide normals reflect the **previous** tick until [method CharacterBody3D.move_and_slide] runs.
	var scrape_vertical_pre_slide: bool = _has_scrape_vertical_contact()
	var scrape_probe_hold: bool = (
		locomotion_probe_floor
		and scrape_vertical_pre_slide
		and _player.velocity.y >= LOCOMOTION_WALL_SCRAPE_PROBE_HOLD_VY_MAX
	)
	if _player.is_on_floor():
		_floor_coyote = LOCOMOTION_FLOOR_COYOTE_TICKS
	elif scrape_probe_hold:
		_floor_coyote = LOCOMOTION_FLOOR_COYOTE_TICKS
	else:
		_floor_coyote = maxi(0, _floor_coyote - 1)
	var locomotion_grounded_y: bool = _player.is_on_floor() or _floor_coyote > 0
	var wish_active_scrape_vertical_pre: bool = wish_probe_active and scrape_vertical_pre_slide
	var scrape_micro_slip_xz: bool = locomotion_grounded_y and wish_active_scrape_vertical_pre
	if locomotion_grounded_y:
		_player.velocity.y = 0.0
	else:
		_player._apply_walk_air_gravity(delta, feet_y, Vector2.ZERO)
	_player._walk_clip_horizontal_velocity_against_vertical_contacts(wish)
	_player.floor_snap_length = _floor_snap_moving
	if locomotion_grounded_y and wish_active_scrape_vertical_pre:
		if locomotion_probe_floor:
			_player.floor_snap_length = 0.0
		else:
			_player.floor_snap_length = minf(
				_floor_snap_moving, LOCOMOTION_WALL_SCRAPE_SNAP_FALLBACK
			)
	_player.move_and_slide()
	if scrape_micro_slip_xz:
		_project_global_xz_on_wish_line_if_micro_slip(scrape_ray_anchor_feet_xz, wish_probe)
	var feet_after: float = _feet_y()
	var scrape_vertical_post_slide: bool = _has_scrape_vertical_contact()
	var on_floor_post_slide: bool = _player.is_on_floor()
	var post_slide_wish_floor_scrape: bool = (
		wish_probe_active and on_floor_post_slide and scrape_vertical_post_slide
	)
	if post_slide_wish_floor_scrape:
		var ray_absorbed: bool = false
		if locomotion_probe_floor:
			var w3d := _player.get_world_3d()
			if w3d != null:
				var probe_feet_y: float = _floor_ray_feet_y_median(
					w3d.direct_space_state, scrape_ray_anchor_feet_xz, wish_probe
				)
				if probe_feet_y != FLOOR_RAY_FEET_INVALID:
					var raw_absorb_dy: float = probe_feet_y - feet_after
					var absorb_dy: float = raw_absorb_dy
					if absf(raw_absorb_dy) > LOCOMOTION_SCRAPE_RAY_FEET_ABSORB_MAX:
						_scrape_probe_y_ema_valid = false
					else:
						if _scrape_probe_y_ema_valid:
							_scrape_probe_y_ema = lerpf(
								_scrape_probe_y_ema,
								probe_feet_y,
								LOCOMOTION_SCRAPE_RAY_PROBE_Y_LERP
							)
						else:
							_scrape_probe_y_ema = probe_feet_y
							_scrape_probe_y_ema_valid = true
						absorb_dy = _scrape_probe_y_ema - feet_after
					if (
						absf(absorb_dy) <= LOCOMOTION_SCRAPE_RAY_FEET_ABSORB_MAX
						and absf(absorb_dy) > 1e-5
					):
						_player.global_position.y += absorb_dy
						feet_after = _feet_y()
						_scrape_feet_y_lock = feet_after
						_scrape_feet_lock_valid = true
						ray_absorbed = true
		if not ray_absorbed:
			if not _scrape_feet_lock_valid:
				_scrape_feet_y_lock = feet_after
				_scrape_feet_lock_valid = true
			else:
				if absf(_scrape_feet_y_lock - feet_after) > LOCOMOTION_SCRAPE_FEET_ERR_GATE:
					_scrape_feet_y_lock = feet_after
				else:
					_scrape_feet_y_lock = lerpf(
						_scrape_feet_y_lock, feet_after, LOCOMOTION_SCRAPE_FEET_LOCK_LERP
					)
				var pull_feet: float = _scrape_feet_y_lock - feet_after
				var step_y: float = clampf(
					pull_feet,
					-LOCOMOTION_SCRAPE_FEET_CORRECT_MAX,
					LOCOMOTION_SCRAPE_FEET_CORRECT_MAX
				)
				if absf(step_y) > 1e-6:
					_player.global_position.y += step_y
		if scrape_micro_slip_xz:
			_project_global_xz_on_wish_line_if_micro_slip(scrape_ray_anchor_feet_xz, wish_probe)
	else:
		reset_scrape_feet_lock()
	if post_slide_wish_floor_scrape:
		if locomotion_probe_floor:
			_align_velocity_xz_to_wish()
		if locomotion_grounded_y:
			_player.velocity.y = 0.0
	_player._snap_capsule_upright()
	_player._debug_trace_transform("physics")