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 global_pose := skeleton.get_bone_global_pose(foot.bone_id)
		var old_position := foot.position
		foot.position = skeleton.to_global(global_pose.origin)
		
		if foot.floor_distance < ik_step_threshold:
			var bone_to_global := skeleton.global_transform * global_pose
			var normal_local := bone_to_global.basis.transposed() * foot.normal
			var pose := skeleton.get_bone_pose(foot.bone_id)
			var rot := get_foot_rotation(pose.basis, normal_local)
			ik_time = minf(ik_time + ik_speed * delta, 1.0)
			skeleton.set_bone_pose_rotation(foot.bone_id, pose.basis.slerp(rot, ik_time))
		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 get_foot_rotation(base: Basis, normal: Vector3) -> Basis:
	var rot := align_with_y(base, normal)
	return rot.rotated(Vector3.UP, PI)

func align_with_y(base: Basis, normal: Vector3) -> Basis:
	base.x = -base.z.cross(normal)
	base.y = normal
	return base.orthonormalized()

# func get_foot_rotation2(base: Basis, normal: Vector3) -> Basis:
# 	normal.x = -normal.x
# 	normal.z = -normal.z
# 	return align_up(base, normal)

# func align_up(base: Basis, normal: Vector3) -> Basis:
# 	var result = Basis()
# 	var node_scale = base.get_scale().abs()

# 	result.x = normal.cross(base.z)
# 	result.y = normal
# 	result.z = base.x.cross(normal)

# 	result = result.orthonormalized()
# 	result.x *= node_scale.x
# 	result.y *= node_scale.y
# 	result.z *= node_scale.z

# 	return result