extends CharacterBody3D

@export var lock_x: bool = false
@export var lock_y: bool = false
@export var lock_z: bool = false
@export var ReCorner_Snap: int = 0
var _recorner_ceiling_flag: bool = false

@export var speed: float = 6.0
@export var jump_speed: float = 5.5

func _physics_process(delta):
	var input_dir = Input.get_vector("ui_left", "ui_right", "ui_up", "ui_down")
	var direction = transform.basis * Vector3(input_dir.x, 0, input_dir.y)

	velocity.x = direction.x * speed
	velocity.z = direction.z * speed

	if is_on_floor() and Input.is_action_just_pressed("ui_accept"):
		velocity.y = jump_speed
	else:
		velocity.y -= ProjectSettings.get_setting("physics/3d/default_gravity") * delta

	ReCorner(delta)
	move_and_slide()

func ReCorner(delta: float) -> void:
	if delta <= 1e-6:
		return
	
	var EPS: float = 1e-4
	var SKIN: float = 0.015
	var MIN_SNAP: float = 0.04
	var MAX_SNAP: float = 0.65
	var MAX_LATERAL: float = 1.2
	var CORNER_CLEARANCE: float = 0.12
	var PROBE_MULT: float = 0.12
	var SAFETY: float = 0.06
	
	var u: Vector3 = up_direction.normalized() if up_direction.length() > EPS else Vector3.UP
	var mask: Vector3 = Vector3(float(not lock_x), float(not lock_y), float(not lock_z))
	
	if mask == Vector3.ZERO:
		return
	
	var v_up: float = velocity.dot(u)
	if v_up <= 0.0:
		_recorner_ceiling_flag = false
		return
	
	var snap: float = clamp(
		float(ReCorner_Snap) * 0.01 if ReCorner_Snap > 0 else v_up * delta + SKIN,
		MIN_SNAP,
		MAX_SNAP
	)
	
	if snap <= EPS:
		return
	
	var probe: float = clamp(min(snap * 1.2, PROBE_MULT), 0.012, PROBE_MULT)
	if not test_move(global_transform, u * probe):
		_recorner_ceiling_flag = false
		return
	
	if _recorner_ceiling_flag:
		return
	
	var v_h: Vector3 = velocity - u * v_up
	var h: Vector3 = v_h * delta if v_h.length() > EPS else Vector3.ZERO
	var h_masked: Vector3 = Vector3(h.x * mask.x, h.y * mask.y, h.z * mask.z)
	
	if not test_move(global_transform.translated(h_masked), u * snap):
		return
	
	var d: Vector3
	if v_h.length() > EPS:
		d = v_h.normalized()
	else:
		var fallback: Vector3 = Vector3.UP if abs(u.y) < 0.99 else Vector3.RIGHT
		d = u.cross(fallback).normalized()
	
	var p: Vector3 = u.cross(d).normalized()
	var best_dist: float = INF
	var best_offset: Vector3 = Vector3.ZERO
	
	var candidates: Array[Vector3] = [d, -d, p, -p]
	var diag_sum: Vector3 = (d + p).normalized()
	var diag_diff: Vector3 = (d - p).normalized()
	if diag_sum.length() > EPS:
		candidates.append(diag_sum)
	if diag_diff.length() > EPS:
		candidates.append(diag_diff)
	if (p - d).length() > EPS:
		candidates.append((p - d).normalized())
	if (-d - p).length() > EPS:
		candidates.append((-d - p).normalized())
	
	for candidate in candidates:
		var dir_check: Vector3 = (candidate - u * candidate.dot(u)).normalized()
		if dir_check.length() <= EPS:
			continue
		
		var dir_m: Vector3 = Vector3(
			candidate.x * mask.x,
			candidate.y * mask.y,
			candidate.z * mask.z
		)
		dir_m = (dir_m - u * dir_m.dot(u)).normalized()
		if dir_m.length() <= EPS:
			continue
		
		for k in [1.5, 2.0, 2.8]:
			var L: float = clamp(k * snap, 0.0, MAX_LATERAL)
			var s_lo: float = 0.0
			var s_hi: float = -1.0
			
			for step_frac in [0.2, 0.4, 0.6, 0.8, 1.0]:
				var s: float = step_frac * L
				if test_move(global_transform.translated(dir_m * s), u * snap):
					s_lo = s
				else:
					s_hi = s
					break
			
			if s_hi < 0.0:
				continue
			
			for _iter in range(4):
				var mid: float = 0.5 * (s_lo + s_hi)
				if test_move(global_transform.translated(dir_m * mid), u * snap):
					s_lo = mid
				else:
					s_hi = mid
			
			var offset: Vector3 = dir_m * (s_hi + SAFETY)
			var verify_pos: Vector3 = global_position + offset
			var verify_t: Transform3D = Transform3D(global_transform.basis, verify_pos)
			
			if test_move(verify_t, u * snap) or test_move(verify_t, Vector3.ZERO) or test_move(verify_t, u * CORNER_CLEARANCE):
				continue
			
			var safe: bool = true
			var perp: Vector3 = u.cross(dir_check).normalized()
			
			for i in range(12):
				var angle: float = (float(i) / 12.0) * TAU
				var rad: Vector3 = (dir_check * cos(angle) + perp * sin(angle) - u * (dir_check * cos(angle) + perp * sin(angle)).dot(u)).normalized() * CORNER_CLEARANCE
				if test_move(verify_t, rad):
					safe = false
					break
			
			if not safe:
				continue
			
			if test_move(verify_t, u * CORNER_CLEARANCE) or test_move(verify_t, u * CORNER_CLEARANCE * 0.7):
				continue
			
			if perp.length() > EPS:
				var half: float = CORNER_CLEARANCE * 0.5
				for diag in [
					dir_check * half + u * half,
					-dir_check * half + u * half,
					perp * half + u * half,
					-perp * half + u * half
				]:
					if test_move(verify_t, diag):
						safe = false
						break
			
			if not safe:
				continue
			
			if offset.length() < best_dist:
				best_dist = offset.length()
				best_offset = offset
	
	if best_dist < INF:
		global_position += best_offset
		if best_offset.length() > 0.01:
			reset_physics_interpolation()
		_recorner_ceiling_flag = true