extends CharacterBody3D

## NS-23: Nav agent suggests waypoints; arrival uses stored server goal only (not agent “finished”).
## `is_navigation_finished()` / internal target can clear early while we are still far from goal.

const MOVE_SPEED: float = 5.0
const ARRIVE_EPS: float = 0.35
## Outside this XZ distance to server goal, use nav corners; inside, steer straight to goal.
const DIRECT_APPROACH_RADIUS: float = 0.85

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)


## After successful move POST + GET: walk to server target without teleporting.
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)


## NS-19 rejection or cancel pending path without moving the body.
func clear_nav_goal() -> void:
	velocity = Vector3.ZERO
	_has_walk_goal = false
	_nav_agent.set_target_position(global_position)


## Boot (and any hard reconcile): teleport to server position and reset agent.
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 _physics_process(_delta: float) -> void:
	if not _has_walk_goal:
		velocity = Vector3.ZERO
		move_and_slide()
		return

	var to_goal: Vector3 = Vector3(
		_auth_walk_goal.x - global_position.x, 0.0, _auth_walk_goal.z - global_position.z
	)
	var dist_goal: float = to_goal.length()
	if dist_goal <= 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:
		velocity = to_goal.normalized() * MOVE_SPEED
		velocity.y = 0.0
		move_and_slide()
		return

	var dir: Vector3
	if dist_goal <= DIRECT_APPROACH_RADIUS:
		dir = to_goal.normalized()
	else:
		var next_path_position: Vector3 = _nav_agent.get_next_path_position()
		var to_next: Vector3 = Vector3(
			next_path_position.x - global_position.x, 0.0, next_path_position.z - global_position.z
		)
		if to_next.length_squared() > 0.0025:
			dir = to_next.normalized()
		else:
			dir = to_goal.normalized()

	velocity = dir * MOVE_SPEED
	velocity.y = 0.0
	move_and_slide()