extends CharacterBody3D

## NS-23: Follow server move target on walkable geometry.
##
## Horizontal velocity toward nav waypoint or goal; `velocity.y` stays 0 except a short **step-off**
## nudge (see `_use_direct_step_down_steering`) so NS-19 bumps can drop without full gravity.
## Move **legality** 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
## Past this, only direct step-down when footprint still touches `ns19_bump` (nav rim paths).
const STEP_OFF_CLOSE_HORIZ: float = 8.0
## Match server `MovementValidation.MaxHorizontalStep` default; no bee-line past this.
const STEP_OFF_MAX_HORIZ_AUTH: float = 18.0
const FLOOR_PROBE_BODY_Y_OFFSET: float = -0.55
## Extra xz rays for step-off; max Y keeps bump support when center ray sees floor first.
const STEP_FOOTPRINT_HALF: float = 0.2
## Downward vy only while step-off bypass is active (not global gravity).
const STEP_OFF_DESCEND_SPEED: float = 1.6
## 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
var _footprint_cache_frame: int = -1
var _footprint_cache: Dictionary = {}

@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_hit(from: Vector3) -> Dictionary:
	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()]
	return space.intersect_ray(q)


func _ray_floor_y_at(from: Vector3) -> float:
	var hit: Dictionary = _ray_floor_hit(from)
	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 _collider_in_ns19_bump(collider: Variant) -> bool:
	var n: Node = collider as Node
	while n:
		if n.is_in_group("ns19_bump"):
			return true
		n = n.get_parent()
	return false


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)


## One ray pass: max floor Y under footprint + whether any hit is an NS-19 bump (`ns19_bump` group).
func _compute_footprint_step_down_state() -> Dictionary:
	var p: Vector3 = global_position
	var h: float = STEP_FOOTPRINT_HALF
	var oxz: Array[Vector3] = [
		Vector3(h, 0.0, 0.0),
		Vector3(-h, 0.0, 0.0),
		Vector3(0.0, 0.0, h),
		Vector3(0.0, 0.0, -h),
		Vector3(h, 0.0, h),
		Vector3(h, 0.0, -h),
		Vector3(-h, 0.0, h),
		Vector3(-h, 0.0, -h),
	]
	var best_y: float = 0.0
	var got: bool = false
	var on_bump: bool = false
	for yoff: float in [0.0, FLOOR_PROBE_BODY_Y_OFFSET]:
		var from_c: Vector3 = p + Vector3(0.0, yoff, 0.0)
		var hit_c: Dictionary = _ray_floor_hit(from_c)
		if not hit_c.is_empty():
			var yc: float = (hit_c.get("position") as Vector3).y
			if not got:
				best_y = yc
				got = true
			else:
				best_y = maxf(best_y, yc)
			if not on_bump and _collider_in_ns19_bump(hit_c.get("collider")):
				on_bump = true
	for d: Vector3 in oxz:
		for yoff2: float in [0.0, FLOOR_PROBE_BODY_Y_OFFSET]:
			var from_o: Vector3 = p + d + Vector3(0.0, yoff2, 0.0)
			var hit_o: Dictionary = _ray_floor_hit(from_o)
			if not hit_o.is_empty():
				var yo: float = (hit_o.get("position") as Vector3).y
				if not got:
					best_y = yo
					got = true
				else:
					best_y = maxf(best_y, yo)
				if not on_bump and _collider_in_ns19_bump(hit_o.get("collider")):
					on_bump = true
	if not got:
		best_y = _floor_y_under_body()
	return {"max_y": best_y, "touches_ns19_bump": on_bump}


func _footprint_step_down_state() -> Dictionary:
	var f: int = Engine.get_physics_frames()
	if f != _footprint_cache_frame:
		_footprint_cache = _compute_footprint_step_down_state()
		_footprint_cache_frame = f
	return _footprint_cache


## Footing under goal xz (surface Y). Server goal.y may be capsule center (e.g. 0.9) or pick surface
## (e.g. 0) — do not compare that directly to `_floor_y_under_body()` (always surface).
func _destination_footing_y() -> float:
	var g: Vector3 = _auth_walk_goal
	var probe_top: float = maxf(global_position.y, g.y) + 4.0
	var probe: Vector3 = Vector3(g.x, probe_top, g.z)
	var y: float = _ray_floor_y_at(probe)
	if y >= probe_top - 0.05:
		return g.y
	return y


func _use_direct_step_down_steering() -> bool:
	var st: Dictionary = _footprint_step_down_state()
	var floor_y: float = float(st["max_y"])
	var on_bump: bool = bool(st["touches_ns19_bump"])
	var dest_floor_y: float = _destination_footing_y()
	if dest_floor_y >= floor_y - STEP_OFF_FLOOR_MARGIN:
		return false
	var drop: float = floor_y - dest_floor_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_AUTH:
		return false
	if on_bump:
		return true
	return hd <= STEP_OFF_CLOSE_HORIZ


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_capsule: float = absf(full_to_goal.y)
	var dest_foot_y: float = _destination_footing_y()
	var fp: Dictionary = _footprint_step_down_state()
	var max_foot_y: float = float(fp["max_y"])
	# Same lip issue as step-off: `_floor_y_under_body()` can read floor early and match `dest_foot_y`
	# while xz is within ARRIVE_EPS — false “arrived”, goal clears. Use footprint max_y.
	var footing_match: bool = absf(max_foot_y - dest_foot_y) <= VERT_ARRIVE_EPS
	if horiz_dist <= ARRIVE_EPS and (vert_err_capsule <= VERT_ARRIVE_EPS or footing_match):
		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 _use_direct_step_down_steering():
		_set_horizontal_velocity_toward(_auth_walk_goal)
		velocity.y = -STEP_OFF_DESCEND_SPEED
		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()