how i handle stairs in my fps

stair handling.gd

var last_velocity = Vector3.ZERO
var slide_collisions = []

func handle_stairs3(slides:Array, start_position:Vector3, rotation, radius:float,ray:RayCast):
	ray.enabled = true
	$StepClear.enabled = true

	if slides.size()>0:
		var selected_slide:KinematicCollision = null
		var slide_positions = []
		var check_slide = false
		for slide in slides:
			slide_positions.append(slide.position)
			#is it higher than the point we are starting from
			if (slide.position.y - start_position.y) <= step_height:
				if slide.position.y >= start_position.y:
					#make sure its outside the collision body
					if radius <= h_distance3d(start_position,slide.position):
						selected_slide = slide
						check_slide = true
#						print(str("step found, break, normal::",slide.normal.y))
						break
		if selected_slide != null:
			var step_check = {
				ray = false,
				above = false,
				below = false,
				direct = false
			}
			var old_ray = {}
			old_ray.position = ray.global_transform.origin
			old_ray.cast_to = ray.cast_to

			var test_position:Vector3 = start_position
			var new_slide = selected_slide.position
			new_slide.y += 0
			test_position.y = new_slide.y

			var margin = .01
			var h_target:Vector3 =  (test_position - (test_position - new_slide)) + ( new_slide - test_position).normalized()* margin

			ray.global_transform.origin = h_target
			ray.global_transform.origin.y = start_position.y
			ray.force_update_transform()
			ray.force_raycast_update()
#			print("{")
#			print(str(["stepray",ray.is_colliding(),OS.get_system_time_msecs()]))
			step_check.ray = ray.is_colliding()
			var ray_collision:Vector3
			if ray.is_colliding():
				ray_collision = ray.get_collision_point()
#				print(ray_collision.y - start_position.y)

				start_position.distance_to(new_slide)
				#check the horizontal hits
				#dead on
				h_target =  (test_position - (test_position - ray_collision)) + ( ray_collision - test_position).normalized()*margin
				var cc:Vector3 = ( h_target - test_position)

#				print(str(["dead on",line_intersects(test_position, test_position + cc.rotated(Vector3.UP,-rotation),$StepClear),OS.get_system_time_msecs()]))
				step_check.direct = line_intersects(test_position, test_position + cc.rotated(Vector3.UP,-rotation),$StepClear)
				#above
				test_position = start_position
				new_slide = ray_collision
				new_slide.y += step_height/2
				test_position.y = new_slide.y
				h_target =  (test_position - (test_position - new_slide)) + ( new_slide - test_position).normalized()*margin
				cc = ( h_target - test_position)
#				print(str(["above",line_intersects(test_position, test_position + cc.rotated(Vector3.UP,-rotation),$StepClear),OS.get_system_time_msecs()]))
				step_check.above = line_intersects(test_position, test_position + cc.rotated(Vector3.UP,-rotation),$StepClear)
				#below
				test_position = start_position
				new_slide = ray_collision
				new_slide.y -= .05
				test_position.y = new_slide.y
				h_target =  (test_position - (test_position - new_slide)) + ( new_slide - test_position).normalized()*margin
				cc = ( h_target - test_position)
#				print(str(["below",line_intersects(test_position, test_position + cc.rotated(Vector3.UP,-rotation),$StepClear),OS.get_system_time_msecs()]))
				step_check.below = line_intersects(test_position, test_position + cc.rotated(Vector3.UP,-rotation),$StepClear)
#				last_step_position = h_target
				if (step_check.direct or step_check.below) == true and !step_check.above:
					if (ray_collision.y - start_position.y) <= step_height:
#						print("step")
#						print("}\r\n")
						last_step_position = ray_collision
						return ray_collision
				if step_check.above:
					return null
#					print("step not clear")
#				print("}\r\n")
	#			ray.global_transform.origin = old_ray.position
	#			ray.cast_to = old_ray.cast_to
	#			ray.force_update_transform()
	#			ray.force_raycast_update()
				ray.enabled = false
				$StepCheck.enabled = false
				return null
	return null
  
  # movement handling
func _physics_process(delta: float) -> void:
	var step_location = null

#	if Vector2(player_velocity.x,player_velocity.z).length() > 0.2:
	step_location = handle_stairs3(slide_collisions,self.global_transform.origin,global_transform.basis.get_euler().y,$CollisionShape.shape.radius,$StepCheck)
	if step_location == null:
		$status.text += str("no step","\r\n")
	else:
		$status.text += str("stepping","\r\n")
		var r_point = ((self.global_transform.origin - step_location).normalized() * $CollisionShape.shape.radius)
		var step_distance = ((self.global_transform.origin - r_point) - step_location)
#		print(str("step distance", step_distance.length()))
		self.global_transform.origin.y = step_location.y
		self.global_transform.origin -= step_distance

		player_velocity = last_velocity + (last_velocity * delta)
		player_velocity.y += player_velocity.length() * delta
		last_step_position = step_location

    last_velocity = player_velocity
	player_velocity = move_and_slide(player_velocity,Vector3.UP,true)

  # store collisions for use next frame
	slide_collisions = []
	var tempi = get_slide_count()
	for i in range(tempi):
		slide_collisions.append(get_slide_collision(i))
	pass
	
# there asre better ways to do this such as: PhysicsDirectSpaceState.intersect_ray https://docs.godotengine.org/en/stable/classes/class_physicsdirectspacestate.html#class-physicsdirectspacestate-method-intersect-ray
func line_intersects(start_point:Vector3, end_point:Vector3,ray:RayCast):
	#https://answers.unity.com/questions/590671/direct-a-raycast-from-point-a-to-point-b.html
	var start:Vector3 = start_point
	var end:Vector3 = end_point
	var dir:Vector3 = end - start
	ray.enabled = true
#	var distance = abs(start.distance_to(end)) #abs(h_distance3d(start_point, end_point))
#	print(distance)
	ray.global_transform.origin = start #+ Vector3(0,.5,0)
	ray.cast_to = dir #.normalized() * distance  #+ Vector3(0,.5,0)
	ray.force_update_transform()
	ray.force_raycast_update()
	return ray.is_colliding()
	pass
	
#gets the horizontal distance between 2 Vector3's ignoring the y Vector
func h_distance3d(Vec1:Vector3,Vec2:Vector3) -> float:
	return Vector2(Vec1.x,Vec1.z).distance_to(Vector2(Vec2.x,Vec2.z))
#	manual way to calculate the distance
#	var vec_sum = (Vec1 - Vec2)
#	var vec_sum_squared = Vector2(pow(vec_sum.x,2),pow(vec_sum.z,2))
#	var distance:float = sqrt(vec_sum_squared.x+vec_sum_squared.y)	
#	return distance