extends CharacterBody3D

## NS-23: Follow server move target on walkable geometry.
##
## Horizontal velocity toward nav waypoint or goal only; `velocity.y` stays 0. Height along ramps
## and steps comes from `move_and_slide` + floor_* on CharacterBody3D. Move **legality** (step,
## distance, bounds) is server MoveCommand only — not reimplemented here.
##
## Nav detours except when goal Y is below us: then xz toward goal (waypoints sit under bump rims).

const MOVE_SPEED: float = 5.0
const ARRIVE_EPS: float = 0.35
const VERT_ARRIVE_EPS: float = 0.055
const DIRECT_APPROACH_RADIUS: float = 0.85
## Server goal clearly below us (descending). Nav mesh waypoints are often under the rim first;
## steering at next gives almost no horizontal speed — walk out on xz toward the authorized goal.
const DESCEND_GOAL_Y_MARGIN: float = 0.06

var _has_walk_goal: bool = false
var _auth_walk_goal: Vector3 = Vector3.ZERO

@onready var _nav_agent: NavigationAgent3D = $NavigationAgent3D


func _ready() -> void:
	_nav_agent.set_target_position(global_position)


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


func clear_nav_goal() -> void:
	velocity = Vector3.ZERO
	_has_walk_goal = false
	_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
	_nav_agent.set_target_position(world_pos)


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_process(_delta: float) -> void:
	if not _has_walk_goal:
		velocity = Vector3.ZERO
		move_and_slide()
		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
		_nav_agent.set_target_position(global_position)
		move_and_slide()
		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)
		move_and_slide()
		return

	if _auth_walk_goal.y < global_position.y - DESCEND_GOAL_Y_MARGIN:
		_set_horizontal_velocity_toward(_auth_walk_goal)
		move_and_slide()
		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)

	move_and_slide()