extends Skeleton3D

@onready var physical_bone_sim = $PhysicalBoneSimulator3D
var physics_bones: Array[Node]
@export var target_skeleton: Skeleton3D = null
# Called when the node enters the scene tree for the first time.

func _ready() -> void:
	physics_bones = physical_bone_sim.get_children().filter(func(x): return x is PhysicalBone3D)
	physical_bone_sim.physical_bones_start_simulation()


# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta: float) -> void:
	pass

func die() -> void:
	for b: PhysicalBone3D in physics_bones:
		#var target_transforms: Vector3 = target_skeleton.global_position * target_skeleton.get_bone_global_pose(b.get_bone_id())
		b.global_position = target_skeleton.to_global(target_skeleton.get_bone_global_pose(b.get_bone_id()).origin)
		#var current_transforn: Transform3D = global_transform * get_bone_global_pose(b.get_bone_id())
		#var position_difference: Vector3 = target_transforms.origin - current_transforn.origin
		#var force: Vector3 = hookes_law(position_difference, b.linear_velocity, 200.0, 40.0)
		#b.linear_velocity += force * delta
		#
		#var rotation_difference: Basis = target_transforms.basis * current_transforn.basis.inverse()
		#var torgue = hookes_law(rotation_difference.get_euler(), b.angular_velocity, 200.0, 40.0)
		#b.angular_velocity += torgue * delta

func hookes_law(dis: Vector3, cur_vel: Vector3, stif: float, damp: float):
	return (stif * dis) - (damp * cur_vel)