class_name FootstepsComponent
extends Node3D

@export var skeleton: Skeleton3D
@export var movement: MovementComponent
@export var footsteps: AudioStreamRandomizer
@export var step_threshold := 0.1
@export var ik_step_threshold := 0.2
@export var ik_speed := 5.0

const feet_names := ["LeftFoot", "RightFoot"]

var feet: Array[Foot] = []
var ik_time: float

func _ready():
	for foot_name in feet_names:
		var foot = Foot.new()
		foot.bone_name = foot_name
		foot.bone_id = skeleton.find_bone(foot_name)
		foot.audio = get_node(foot_name)
		foot.position = skeleton.to_global(skeleton.get_bone_global_pose(foot.bone_id).origin)
		foot.floor_distance = INF
		foot.normal = Vector3.UP
		foot.attachment = BoneAttachment3D.new()
		foot.attachment.bone_name = foot_name
		foot.attachment.bone_idx = foot.bone_id
		foot.audio.reparent(foot.attachment)
		skeleton.add_child(foot.attachment)
		feet.append(foot)

func _process(delta: float):
	for foot in feet:
		var old_position := foot.position
		var global_pose := skeleton.get_bone_global_pose(foot.bone_id)
		var world_pose := skeleton.global_transform * global_pose
		foot.position = world_pose.origin

		if foot.floor_distance < ik_step_threshold:
			ik_time = minf(ik_time + ik_speed * delta, 1.0)
			align_surface_normal(foot, world_pose)
		else:
			ik_time = 0

		if foot.floor_distance > step_threshold:
			continue
		
		foot.velocity = (foot.position - old_position) / delta
		
		if foot.velocity.y > 0:
			continue
		
		if foot.velocity.length_squared() < 1.0:
			continue
		
		if foot.audio.playing && foot.audio.get_playback_position() < 0.3:
			continue
		
		play(foot.audio)

func _physics_process(_delta):
	var space_state = get_world_3d().direct_space_state

	for foot in feet:
		var from := foot.position
		var to := foot.position + Vector3(0, -10, 0)
		var query = PhysicsRayQueryParameters3D.create(from, to, 1)
		var result = space_state.intersect_ray(query)
		
		if result:
			foot.floor_distance = (foot.position - result.position).length()
			foot.normal = result.normal
		else:
			foot.floor_distance = INF
			foot.normal = Vector3.UP

func play(audio_player: AudioStreamPlayer3D):
	audio_player.stream = footsteps
	audio_player.play()

func align_surface_normal(foot: Foot, world_pose: Transform3D):
	var pose := skeleton.get_bone_pose(foot.bone_id)
	var local_normal := world_pose.basis.transposed() * foot.normal
	var new_basis := get_foot_rotation(pose.basis, local_normal)
	skeleton.set_bone_pose_rotation(foot.bone_id, pose.basis.slerp(new_basis, ik_time))

func get_foot_rotation(old: Basis, normal: Vector3) -> Basis:
	var new := Basis(-old.x, normal, old.z)
	new = new.orthonormalized()
	new = new.rotated(Vector3.UP, PI)
	return new