extends Node3D

@onready var face_mesh_instance: MeshInstance3D = $Armature/Skeleton3D/face
@onready var skeleton: Skeleton3D = $Armature/Skeleton3D
@export var neck_bone_name:String = 'head'
@onready var anim_tree: AnimationTree = $AnimationTree

var neck_bone:int
var base_neck_rot:Quaternion


func _ready() -> void:
	ARKitSingleton._server.change_port(11111)
	ARKitSingleton._server.start()

	neck_bone = skeleton.find_bone(neck_bone_name)
	base_neck_rot = skeleton.get_bone_pose_rotation(neck_bone)


func _process(_delta: float) -> void:
	var first_subject: ARKitSubject

	# Check if at least one subject (ARKit UDP service screaming at you) exists
	if len(ARKitSingleton._server.subjects.keys()) == 0:
		return

	# Select the first subject like a brute
	for subject: ARKitSubject in ARKitSingleton._server.subjects.values():
		first_subject = subject
		break

	# Change each blendshape of your model
	for i in range(first_subject.packet.number_of_blendshapes):
		var blendshape_name: String = ARKitServer.blendshape_string_mapping[i]
		var blendshape_value: float = first_subject.packet.blendshapes_array[i]


		# The classic 52 blendshapes of the face
		if i <= ARKitServer.BlendShape.TONGUE_OUT:
			set_blend_shape(face_mesh_instance, blendshape_name, blendshape_value)
		else:
			break
		
		# The blendshapes related to the head rotation
		var head_yaw: float = first_subject.packet.blendshapes_array[ARKitServer.BlendShape.HEAD_YAW]
		var head_pitch: float = first_subject.packet.blendshapes_array[ARKitServer.BlendShape.HEAD_PITCH]
		var head_roll: float = first_subject.packet.blendshapes_array[ARKitServer.BlendShape.HEAD_ROLL]
		skeleton.set_bone_pose_rotation(neck_bone, base_neck_rot * Quaternion.from_euler(Vector3(-head_pitch, -head_yaw, -head_roll)))
	
		# The ones related to eyes rotation (It's hacky here, both eyes behave the same, so you can't be crosse-eyed). Do better
		var left_eye_yaw: float = first_subject.packet.blendshapes_array[ARKitServer.BlendShape.LEFT_EYE_YAW]
		var left_eye_pitch: float = first_subject.packet.blendshapes_array[ARKitServer.BlendShape.LEFT_EYE_PITCH]
		var eyes_pos := Vector2(left_eye_yaw*PI/2, -left_eye_pitch*PI/2)
		anim_tree.set('parameters/blend_position', eyes_pos)


static func set_blend_shape(mesh_instance: MeshInstance3D, blendshape_name: String, blendshape_value: float) -> void:
	var new_name: String = lowercase_first_letter(blendshape_name)
	var blend_shape_idx: int = mesh_instance.find_blend_shape_by_name(new_name)
	if blend_shape_idx == -1:
		push_warning("blendshape: ", new_name, " not found in node: ", mesh_instance)
		return # Not found
	mesh_instance.set_blend_shape_value(blend_shape_idx, blendshape_value)


# Because the blendshapes names on Unreal seems to be Uppercased I used Upperased ones in my enums. Meta says that it's not uppercased, but LLF use Uppercased ones in the debug settings, what to do?
static func lowercase_first_letter(text: String) -> String:
	pass