extends Node3D

const PLAYER_SIZE: float = 0.4
const SPEED: float = 25.

@export var map: Node3D
@export var camera: FollowCamera

var facing = Vector2(1, 0)
var velocity = Vector2(0, 0)

func _physics_process(delta):
	var input = Vector2(Input.get_axis("left", "right"), Input.get_axis("forward", "backwards")).normalized()
	input = input.rotated(-camera.angle_target)

	if input.length() > 0.1:
		facing = lerp_vector2_exp(facing, input, delta * 10.)

	self.rotation.y = facing.angle()

	velocity.x += input.x * delta * 0.5
	velocity.y += input.y * delta * 0.5

	position = Vector3(
		position.x + (velocity.x * delta * SPEED),
		0,
		position.z + (velocity.y * delta * SPEED)
	)

	# collide
	collide(delta)

	velocity = lerp_vector2_exp(velocity, Vector2(0, 0), delta * 5.)

func collide(_delta: float):
	for i in map.get_children():
		if is_instance_of(i, FullTile):
			var tile: FullTile = i
			var d = aabb_circle_distance(
				tile.position.x,
				tile.position.z,
				tile.position.x + 1,
				tile.position.z + 1,
				position.x,
				position.z
			)

			if d > PLAYER_SIZE:
				continue

			var h = 0.01
			var d_sample_x = aabb_circle_distance(
				tile.position.x,
				tile.position.z,
				tile.position.x + 1,
				tile.position.z + 1,
				position.x + h,
				position.z
			)
			var d_sample_y = aabb_circle_distance(
				tile.position.x,
				tile.position.z,
				tile.position.x + 1,
				tile.position.z + 1,
				position.x,
				position.z + h
			)
			var grad_x = (d_sample_x - d) / h
			var grad_y = (d_sample_y - d) / h

			position.x += (PLAYER_SIZE - d) * grad_x
			position.z += (PLAYER_SIZE - d) * grad_y

			var vdotn = (grad_x * velocity.x) + (grad_y * velocity.y)

			velocity.x -= grad_x * vdotn
			velocity.y -= grad_y * vdotn
	# TODO: Player collisions

func lerp_vector2_exp(current: Vector2, target: Vector2, delta: float) -> Vector2:
	return Vector2(
		target.x + (current.x - target.x) * exp( - delta),
		target.y + (current.y - target.y) * exp( - delta)
	)

func aabb_circle_distance(
	min_x: float,
	min_y: float,
	max_x: float,
	max_y: float,
	px: float,
	py: float
) -> float:
	var dx = px - max(min_x, min(max_x, px))
	var dy = py - max(min_y, min(max_y, py))
	return sqrt(dx * dx + dy * dy)