extends CharacterBody3D

## NS-23: Nav agent suggests waypoints; arrival uses stored server goal only (not agent “finished”).
## NS-19 bumps: **autostep** via `test_move` — up+forward to mount, down+forward to leave (pure down
## hits the bump volume you’re standing on).

const MOVE_SPEED: float = 5.0
const ARRIVE_EPS: float = 0.35
const VERT_ARRIVE_EPS: float = 0.16
const MAX_CLIMB_SPEED: float = 2.6
const MAX_DESCENT_SPEED: float = 2.2
const DIRECT_APPROACH_RADIUS: float = 0.85
const STEP_MAX_HEIGHT: float = 0.32
const STEP_PROBE_FWD: float = 0.38
const STUCK_HORIZ_SPEED: float = 0.28
## Drop tries when leaving a bump (combined with forward over the lip).
const LEDGE_DROP_STEPS: PackedFloat32Array = PackedFloat32Array([0.08, 0.12, 0.18, 0.24, 0.32])

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 _steer_toward_world_point(point: Vector3) -> void:
	var delta: Vector3 = point - global_position
	if delta.length_squared() < 1e-8:
		velocity = Vector3.ZERO
		return
	var desired: Vector3 = delta.normalized() * MOVE_SPEED
	desired.y = clampf(desired.y, -MAX_DESCENT_SPEED, MAX_CLIMB_SPEED)
	velocity = desired


## Godot `test_move`: returns **true** if the motion would hit something (blocked).
func _try_autostep(delta: float, full_to_goal: Vector3) -> bool:
	var to_goal_h: Vector3 = Vector3(
		full_to_goal.x, 0.0, full_to_goal.z
	)
	if to_goal_h.length_squared() < 0.0225:
		return false
	var dir_h: Vector3 = to_goal_h.normalized()
	var fwd_len: float = maxf(MOVE_SPEED * delta, 0.1)
	var motion_fwd: Vector3 = dir_h * minf(fwd_len, STEP_PROBE_FWD)
	var xf: Transform3D = global_transform
	if not test_move(xf, motion_fwd):
		return false
	if full_to_goal.y > 0.02:
		return _autostep_up(xf, motion_fwd, dir_h)
	if full_to_goal.y < -0.02:
		return _autostep_down(xf, motion_fwd, dir_h)
	return false


func _autostep_up(xf: Transform3D, motion_fwd: Vector3, dir_h: Vector3) -> bool:
	var motion_up: Vector3 = Vector3(0.0, STEP_MAX_HEIGHT, 0.0)
	if test_move(xf, motion_up):
		return false
	var xf_up := Transform3D(xf.basis, xf.origin + motion_up)
	if test_move(xf_up, motion_fwd):
		return false
	global_position += motion_up + motion_fwd
	velocity.x = dir_h.x * MOVE_SPEED
	velocity.z = dir_h.z * MOVE_SPEED
	velocity.y = 0.0
	return true


func _autostep_down(xf: Transform3D, motion_fwd: Vector3, dir_h: Vector3) -> bool:
	for i: int in range(LEDGE_DROP_STEPS.size()):
		var drop: float = LEDGE_DROP_STEPS[i]
		var motion_off: Vector3 = motion_fwd + Vector3(0.0, -drop, 0.0)
		if test_move(xf, motion_off):
			continue
		global_position += motion_off
		velocity.x = dir_h.x * MOVE_SPEED
		velocity.z = dir_h.z * MOVE_SPEED
		velocity.y = -minf(MAX_DESCENT_SPEED, 1.2)
		return true
	return false


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:
		_steer_toward_world_point(_auth_walk_goal)
		move_and_slide()
		_maybe_autostep_after_slide(delta, full_to_goal, horiz_dist)
		return

	if horiz_dist <= DIRECT_APPROACH_RADIUS:
		_steer_toward_world_point(_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:
			_steer_toward_world_point(next_path_position)
		else:
			_steer_toward_world_point(_auth_walk_goal)

	move_and_slide()
	_maybe_autostep_after_slide(delta, full_to_goal, horiz_dist)


func _maybe_autostep_after_slide(
	delta: float, full_to_goal: Vector3, horiz_dist: float
) -> void:
	if not _has_walk_goal:
		return
	if not is_on_floor() and not is_on_wall():
		return
	var hspd: float = Vector2(velocity.x, velocity.z).length()
	var want_step: bool = hspd < STUCK_HORIZ_SPEED
	if not want_step and is_on_wall():
		want_step = horiz_dist > 0.12 and (
			full_to_goal.y > 0.03 or full_to_goal.y < -0.03
		)
	if want_step and _try_autostep(delta, full_to_goal):
		move_and_slide()