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 for detours; short local step-down (feet vs goal Y, not body origin) may bypass waypoints.

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
const FLOOR_PROBE_DOWN: float = 4.0
const STEP_OFF_FLOOR_MARGIN: float = 0.04
## Only NS-19-sized step downs (not cliffs); allows a wider horiz cap without bee-lining big drops.
const STEP_OFF_MIN_DROP: float = 0.055
const STEP_OFF_MAX_DROP: float = 0.24
const STEP_OFF_MAX_HORIZ: float = 3.0
const FLOOR_PROBE_BODY_Y_OFFSET: float = -0.55
## If next waypoint is within this xz of us, steer to goal (rim / underfoot waypoints).
const NEXT_WAYPOINT_MIN_HORIZ_SQ: float = 0.01

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 _ray_floor_y_at(from: Vector3) -> float:
	var space: PhysicsDirectSpaceState3D = get_world_3d().direct_space_state
	var to: Vector3 = from + Vector3(0.0, -FLOOR_PROBE_DOWN, 0.0)
	var q: PhysicsRayQueryParameters3D = PhysicsRayQueryParameters3D.create(from, to)
	q.collision_mask = 1
	q.exclude = [get_rid()]
	var hit: Dictionary = space.intersect_ray(q)
	if hit.is_empty():
		return from.y
	var pv: Variant = hit.get("position")
	if pv is Vector3:
		return (pv as Vector3).y
	return from.y


func _floor_y_under_body() -> float:
	var y_center: float = _ray_floor_y_at(global_position)
	var lower: Vector3 = global_position + Vector3(0.0, FLOOR_PROBE_BODY_Y_OFFSET, 0.0)
	var y_lower: float = _ray_floor_y_at(lower)
	return maxf(y_center, y_lower)


func _nav_short_step_down_use_goal_direct() -> bool:
	var floor_y: float = _floor_y_under_body()
	if _auth_walk_goal.y >= floor_y - STEP_OFF_FLOOR_MARGIN:
		return false
	var drop: float = floor_y - _auth_walk_goal.y
	if drop < STEP_OFF_MIN_DROP or drop > STEP_OFF_MAX_DROP:
		return false
	var hd: float = Vector2(
		_auth_walk_goal.x - global_position.x, _auth_walk_goal.z - global_position.z
	).length()
	if hd > STEP_OFF_MAX_HORIZ:
		return false
	return true


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 _nav_short_step_down_use_goal_direct():
		_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() > NEXT_WAYPOINT_MIN_HORIZ_SQ:
			_set_horizontal_velocity_toward(next_path_position)
		else:
			_set_horizontal_velocity_toward(_auth_walk_goal)

	move_and_slide()