Trying to get a grid based movement in 3D

So, I have it working better, there are still some bugs, every now and then it gets stuck on a wall, trying to debug it to figure out why, I have the ShapeCast just slightly smaller than the character size so it checks to area that the character will soon be occupying. I'll keep working at the bugs, here is the code that I have thus far.

extends CharacterBody3D


const SPEED = 5

# Used as movement perameters
var moving : bool
var move_queue : Array
var current_move := Vector3.ZERO
var start_pos := Vector3.ZERO
var direction : Vector3

# Used for detecting collisions for movement
@onready var ray := $RayCast3D

func _ready() -> void:
	apply_floor_snap()

# Controls populating move_queue
func _input(event: InputEvent) -> void:
	var move_action := {
		"north":Vector3(-1,0,0),
		"south":Vector3(1,0,0),
		"east":Vector3(0,0,-1),
		"west":Vector3(0,0,1),
		}
	for action in move_action:
		if event.is_action_pressed(action): 
			# Checks if this is a duplicate; Moves input to front of queue
			if not move_queue.has(move_action[action]): 
				move_queue.push_front(move_action[action])
				if not current_move:
					current_move = move_action[action]
				break
			# Checks if event is input release; removes movement from queue
		elif event.is_action_released(action): 
			move_queue.erase(move_action[action])
			break

# Controls gridbased movement at 1 meter increments
func _physics_process(delta: float) -> void:
	# Gravity
	if not is_on_floor():
		velocity += get_gravity() * delta
		move_and_slide()
	# Sets direction, and shapecast direction
	direction = (transform.basis * Vector3(current_move.x, 0, current_move.z)).normalized()
	ray.set_target_position(Vector3(direction.x,0,direction.z))
	ray.force_shapecast_update()
	# Checks to see if there is a collision in direction
	if ray.is_colliding() and not moving:
		# Checks to see if move_queue has an entry; Sets new direction; Removes current move from queue
		if move_queue.size() >= 2:
			move_queue.erase(current_move)
			current_move = move_queue.front()
			direction = (transform.basis * Vector3(current_move.x, 0, current_move.z)).normalized()
			velocity.x = direction.x * SPEED
			velocity.z = direction.z * SPEED
			moving = false
		# If no new direction resets direction to ZERO
		else:
			move_queue.erase(current_move)
			current_move = Vector3.ZERO
			moving = false
	elif current_move:
		velocity.x = direction.x * SPEED
		velocity.z = direction.z * SPEED
		moving = true
	# Checks to confirm that current_move isn't ZERO
	else:
		moving = false

	# Guards movement from repeating
	if moving:
		# Sets starting position of movement for calculating end of movement
		if not start_pos:
			start_pos = position
		# Checks that move distance hasn't been completed
		if start_pos.distance_to((position + current_move)) <= 2:
			move_and_slide()
		# Snaps position to meter increment, and resets start_pos, and sets new current_move
		else:
			start_pos = Vector3.ZERO
			if not move_queue.is_empty():
				current_move = move_queue[0]
			else:
				current_move = Vector3.ZERO
			moving = false
		position.snapped(Vector3(1,1,1)) 

Edit: I realized I did a stupid and didn't actually apply the snapping to the position, I needed to go position = position.snapped(Vector3(1,1,1) but I didn't haha. So, I added some improvements and so far the bugs haven't shown up in my testing, and it also allows for smoother scaling with speed.

	if moving:
		# Sets starting position of movement for calculating end of movement
		if not start_pos:
			start_pos = position
		# Checks that move distance hasn't been completed
		if start_pos.distance_to((position + current_move)) <= 2:
			move_and_slide()
			position = position.snapped(Vector3(SPEED * 0.01,SPEED * 0.01,SPEED * 0.01)) 
		# Snaps position to meter increment, and resets start_pos, and sets new current_move
		else:
			position = position.snapped(Vector3(1,1,1)) 
			start_pos = Vector3.ZERO
			if not move_queue.is_empty():
				current_move = move_queue[0]
			else:
				current_move = Vector3.ZERO
			moving = false

xyz So for this would you suggest I just have an array of occupied locations that I search before moving?

Array [occupied location]
If not array.has[position + dir]
Array.append[position + dir]
Array.erase[position]
Move

Something like this?

Also, would you suggest having an astar3d to handle terrain, so that only non terrain objects need to be in the array. This would shrink the size of the array when checking for occupancy.