extends CharacterBody3D

## NS-23 path-follow; NS-24 idle. Details in `client/README.md` (movement, idle).
##
## Summary: Jolt, no interp; dual floor_max_angle; rim/straddle, bump lip escape.
## Snap upright after motion. Do not set global_transform in _process.
## Walk assist may run a second move_and_slide().

const MOVE_SPEED: float = 5.0
## Moving: floor angle for seams onto stepped QA bumps (~50°).
const FLOOR_MAX_ANGLE_MOVING_DEG: float = 50.0
## Idle: tighter angle to reduce vertex / vertical-face flicker (~35°).
const FLOOR_MAX_ANGLE_IDLE_DEG: float = 35.0
## Physics ticks to keep moving floor angle after walk goal clears (arrival).
const FLOOR_ANGLE_LOOSE_TICKS_AFTER_STOP: int = 96
const ARRIVE_EPS: float = 0.35
const VERT_ARRIVE_EPS: float = 0.055
const DIRECT_APPROACH_RADIUS: float = 0.85
const FLOOR_SNAP_MOVING: float = 0.32
## Idle floor snap length (stronger to pin rim contacts).
const FLOOR_SNAP_IDLE: float = 0.11
## Below this floor-normal dot up, idle tick uses **moving** `floor_max_angle` (rim).
const IDLE_RIM_MIN_FLOOR_UP_DOT: float = 0.968
## Horizontal nudge per tick for rim / straddle settle (**`_maybe_idle_rim_settle_nudge`**).
const IDLE_RIM_SETTLE_STEP: float = 0.004
const DESCEND_GOAL_Y_MARGIN: float = 0.06
## Y lift when blocked by wall-ish slide but nav goal is higher (stepped bumps).
const WALK_STEP_ASSIST_DELTA: float = 0.11
## Skip assist when horizontal velocity already aligns enough with want-dir (corners slide slowly).
const WALK_STEP_ASSIST_MAX_FORWARD_DOT: float = 0.52
const WALK_STEP_ASSIST_COOLDOWN_TICKS: int = 8
## Capsule height 1.0 in scene: origin at center, feet ~ **`y - 0.5`**.
const CAPSULE_HALF_HEIGHT: float = 0.5
const BUMP_COLLISION_CONSTS_SCRIPT: Script = preload(
	"res://scripts/random_floor_bump_collision_constants.gd"
)
## Idle radial push per tick off bump lip / wall (meters; 120 Hz physics).
const IDLE_BUMP_ESCAPE_STEP: float = 0.014
## Player capsule radius; wall touch can happen past axis distance **`col_r`**.
const PLAYER_CAPSULE_RADIUS: float = 0.4

var _has_walk_goal: bool = false
var _auth_walk_goal: Vector3 = Vector3.ZERO
var _floor_angle_loose_ticks: int = 0
var _step_assist_cooldown: int = 0

@onready var _nav_agent: NavigationAgent3D = $NavigationAgent3D


func _ready() -> void:
	physics_interpolation_mode = Node.PHYSICS_INTERPOLATION_MODE_OFF
	_nav_agent.avoidance_enabled = false
	_nav_agent.set_target_position(global_position)
	floor_block_on_wall = true


func set_authoritative_nav_goal(world_pos: Vector3) -> void:
	_auth_walk_goal = world_pos
	_has_walk_goal = true
	_floor_angle_loose_ticks = 0
	_nav_agent.set_target_position(world_pos)


func clear_nav_goal() -> void:
	velocity = Vector3.ZERO
	_has_walk_goal = false
	_floor_angle_loose_ticks = FLOOR_ANGLE_LOOSE_TICKS_AFTER_STOP
	_nav_agent.set_target_position(global_position)


func snap_to_server(world_pos: Vector3) -> void:
	global_position = world_pos
	velocity = Vector3.ZERO
	_has_walk_goal = false
	_floor_angle_loose_ticks = 0
	_step_assist_cooldown = 0
	_nav_agent.set_target_position(world_pos)
	reset_physics_interpolation()


func _step_assist_has_support() -> bool:
	if is_on_floor():
		return true
	for i: int in get_slide_collision_count():
		if get_slide_collision(i).get_normal().y > 0.35:
			return true
	return false


func _step_assist_wallish_blocks(want_dir_xz: Vector3) -> bool:
	if is_on_wall():
		return true
	var w2 := Vector2(want_dir_xz.x, want_dir_xz.z)
	if w2.length_squared() < 1e-8:
		return false
	w2 = w2.normalized()
	for i: int in get_slide_collision_count():
		var n: Vector3 = get_slide_collision(i).get_normal()
		if n.y > 0.52:
			continue
		var n2 := Vector2(n.x, n.z)
		if n2.length_squared() < 1e-8:
			continue
		n2 = n2.normalized()
		if n2.dot(w2) < -0.04:
			return true
	return false


func _step_assist_can_raise_by(dy: float) -> bool:
	if dy <= 0.0001:
		return false
	return not test_move(global_transform, Vector3(0.0, dy, 0.0), null, safe_margin)


func _try_walk_step_assist() -> bool:
	if not _has_walk_goal or _auth_walk_goal.y < global_position.y + 0.025:
		return false
	if not _step_assist_has_support():
		return false
	var pos: Vector3 = global_position
	var to_h: Vector3 = Vector3(_auth_walk_goal.x - pos.x, 0.0, _auth_walk_goal.z - pos.z)
	if to_h.length_squared() < 0.03:
		return false
	var want: Vector3 = to_h.normalized()
	var vel_h: Vector3 = Vector3(velocity.x, 0.0, velocity.z)
	if not _step_assist_wallish_blocks(want) or vel_h.dot(want) > WALK_STEP_ASSIST_MAX_FORWARD_DOT:
		return false
	var lift: float = minf(WALK_STEP_ASSIST_DELTA, _auth_walk_goal.y - global_position.y + 0.45)
	lift = minf(lift, maxf(0.0, _auth_walk_goal.y + 0.55 - global_position.y))
	if lift <= 0.002 or not _step_assist_can_raise_by(lift):
		return false
	global_position.y += lift
	return true


func _after_walk_move_and_slide() -> void:
	if _step_assist_cooldown > 0:
		_step_assist_cooldown -= 1
	if _step_assist_cooldown > 0:
		return
	if _try_walk_step_assist():
		_step_assist_cooldown = WALK_STEP_ASSIST_COOLDOWN_TICKS
		floor_snap_length = FLOOR_SNAP_MOVING
		move_and_slide()


func _set_horizontal_velocity_toward(point: Vector3) -> void:
	var pos := global_position
	var dh: Vector3 = Vector3(point.x - pos.x, 0.0, point.z - pos.z)
	if dh.length_squared() < 1e-10:
		dh = Vector3(1.0, 0.0, 0.0)
	velocity = dh.normalized() * MOVE_SPEED
	velocity.y = 0.0


func _physics_idle_tick() -> void:
	velocity = Vector3.ZERO
	floor_snap_length = FLOOR_SNAP_IDLE
	move_and_slide()


func _idle_ridged_floor_contacts() -> bool:
	var upish := 0
	var wallish := 0
	for i: int in get_slide_collision_count():
		var ny: float = get_slide_collision(i).get_normal().y
		if ny > 0.65:
			upish += 1
		elif ny < 0.45:
			wallish += 1
	return upish >= 1 and wallish >= 1


func _maybe_idle_rim_settle_nudge() -> void:
	if get_slide_collision_count() < 1:
		return
	var shallow_floor: bool = (
		is_on_floor() and get_floor_normal().dot(Vector3.UP) < IDLE_RIM_MIN_FLOOR_UP_DOT
	)
	if not shallow_floor and not _idle_ridged_floor_contacts():
		return
	var h := Vector3.ZERO
	if shallow_floor:
		h = Vector3(get_floor_normal().x, 0.0, get_floor_normal().z)
	if h.length_squared() < 1e-12:
		for i: int in get_slide_collision_count():
			var n: Vector3 = get_slide_collision(i).get_normal()
			if n.y < 0.5:
				var nh: Vector3 = Vector3(n.x, 0.0, n.z)
				if nh.length_squared() > h.length_squared():
					h = nh
	if h.length_squared() < 1e-12:
		return
	var step: Vector3 = -h.normalized() * IDLE_RIM_SETTLE_STEP
	if test_move(global_transform, step, null, safe_margin):
		return
	global_position += step


func _maybe_idle_bump_proximity_escape() -> void:
	var feet_y: float = global_position.y - CAPSULE_HALF_HEIGHT
	for node: Node in get_tree().get_nodes_in_group(
		BUMP_COLLISION_CONSTS_SCRIPT.RANDOM_FLOOR_BUMP_MESH_GROUP
	):
		if not node is MeshInstance3D:
			continue
		var mi := node as MeshInstance3D
		var mesh: Mesh = mi.mesh
		if not mesh is CylinderMesh:
			continue
		var cyl := mesh as CylinderMesh
		var c: Vector3 = mi.global_position
		var r_mesh: float = cyl.top_radius
		var h: float = cyl.height
		var col_r: float = minf(
			r_mesh + BUMP_COLLISION_CONSTS_SCRIPT.COLLISION_RADIUS_EXTRA,
			BUMP_COLLISION_CONSTS_SCRIPT.COLLISION_RADIUS_MAX
		)
		var dx: float = global_position.x - c.x
		var dz: float = global_position.z - c.z
		var d: float = sqrt(dx * dx + dz * dz)
		if d < 1e-5:
			continue
		var bottom_y: float = c.y - h * 0.5
		var top_y: float = c.y + h * 0.5
		if feet_y < bottom_y - 0.22 or feet_y > top_y + 0.4:
			continue
		var radial: Vector3 = Vector3(dx / d, 0.0, dz / d)
		var away: Vector3 = radial * IDLE_BUMP_ESCAPE_STEP
		var wall_band_outer: float = col_r + PLAYER_CAPSULE_RADIUS + 0.22
		# Outside visual disc but inside fat collision — clear the invisible lip.
		if d > r_mesh * 0.9 and d < col_r + 0.32:
			if not test_move(global_transform, away, null, safe_margin):
				global_position += away
			return
		# On / above the disc, hugging the **visual** rim — step onto open slab.
		if (
			feet_y >= bottom_y - 0.06
			and feet_y <= top_y + 0.1
			and d >= r_mesh * 0.86
			and d <= r_mesh + 0.14
		):
			if not test_move(global_transform, away, null, safe_margin):
				global_position += away
			return
		# Beside vertical cylinder: axis distance may exceed **`col_r`** while capsule still touches wall.
		if (
			feet_y <= top_y + 0.14
			and d > r_mesh * 0.82
			and d < wall_band_outer
			and (is_on_wall() or _idle_ridged_floor_contacts())
		):
			if not test_move(global_transform, away, null, safe_margin):
				global_position += away
			return


func _snap_capsule_upright() -> void:
	# TODO(NS-24 follow-on): With facing yaw, clear pitch/roll only; keep Y rotation.
	# Full **`Basis.IDENTITY`** would erase look direction.
	var p: Vector3 = global_position
	global_transform = Transform3D(Basis.IDENTITY, p)


func _physics_process(_delta: float) -> void:
	var use_loose_floor_angle: bool = _has_walk_goal or _floor_angle_loose_ticks > 0
	if not _has_walk_goal:
		var floor_up_dot: float = get_floor_normal().dot(Vector3.UP)
		var shallow_idle_floor: bool = is_on_floor() and floor_up_dot < IDLE_RIM_MIN_FLOOR_UP_DOT
		if shallow_idle_floor or _idle_ridged_floor_contacts():
			use_loose_floor_angle = true
	floor_max_angle = deg_to_rad(
		FLOOR_MAX_ANGLE_MOVING_DEG if use_loose_floor_angle else FLOOR_MAX_ANGLE_IDLE_DEG
	)
	if not _has_walk_goal:
		_physics_idle_tick()
		_maybe_idle_rim_settle_nudge()
		_maybe_idle_bump_proximity_escape()
		if _floor_angle_loose_ticks > 0:
			_floor_angle_loose_ticks -= 1
		_snap_capsule_upright()
		return

	var full_to_goal: Vector3 = _auth_walk_goal - global_position
	var horiz_dist: float = Vector2(full_to_goal.x, full_to_goal.z).length()
	var vert_err: float = absf(full_to_goal.y)
	if horiz_dist <= ARRIVE_EPS and vert_err <= VERT_ARRIVE_EPS:
		velocity = Vector3.ZERO
		_has_walk_goal = false
		_floor_angle_loose_ticks = FLOOR_ANGLE_LOOSE_TICKS_AFTER_STOP
		_nav_agent.set_target_position(global_position)
		_physics_idle_tick()
		_maybe_idle_rim_settle_nudge()
		_maybe_idle_bump_proximity_escape()
		_snap_capsule_upright()
		return

	var nav_map: RID = _nav_agent.get_navigation_map()
	if NavigationServer3D.map_get_iteration_id(nav_map) == 0:
		_set_horizontal_velocity_toward(_auth_walk_goal)
		floor_snap_length = FLOOR_SNAP_MOVING
		move_and_slide()
		_after_walk_move_and_slide()
		_snap_capsule_upright()
		return

	if _auth_walk_goal.y < global_position.y - DESCEND_GOAL_Y_MARGIN:
		_set_horizontal_velocity_toward(_auth_walk_goal)
		floor_snap_length = FLOOR_SNAP_MOVING
		move_and_slide()
		_after_walk_move_and_slide()
		_snap_capsule_upright()
		return

	if horiz_dist <= DIRECT_APPROACH_RADIUS:
		_set_horizontal_velocity_toward(_auth_walk_goal)
	else:
		var next_path_position: Vector3 = _nav_agent.get_next_path_position()
		var to_next_h: Vector3 = Vector3(
			next_path_position.x - global_position.x, 0.0, next_path_position.z - global_position.z
		)
		if to_next_h.length_squared() > 0.0025:
			_set_horizontal_velocity_toward(next_path_position)
		else:
			_set_horizontal_velocity_toward(_auth_walk_goal)

	floor_snap_length = FLOOR_SNAP_MOVING
	move_and_slide()
	_after_walk_move_and_slide()
	_snap_capsule_upright()