extends Node3D

## Rotation of cyclic stick model for full control deflection, in degrees
@export var range_cyclic: float = 30

## Main rotor speed, in revolutions per minute
@export var rotor_rpm: float = 500

## Current angle of rotor rotation
var _rotor_azimuth: float = 0

@onready var connector: MarshConnector = $MarshConnector
@onready var skeleton: Skeleton3D = $"Mi-2/Armature/Skeleton3D"
@onready var attitude_root: Node3D = $AttitudeRoot

@onready var bone_cg: int = skeleton.find_bone("BodyCG")
@onready var bone_cyclic: int = skeleton.find_bone("Cyclic")
@onready var bone_rotor: int = skeleton.find_bone("Rotor")

func _process(delta: float) -> void:
	var target := connector.get_aircraft()
	position = target.origin
	
	# Add the rotation to the correct bone for rotation around CG
	var rest := skeleton.get_bone_rest(bone_cg).basis.get_rotation_quaternion()
	var attitude := target.basis.get_rotation_quaternion()
	skeleton.set_bone_pose_rotation(bone_cg, attitude * rest)

	# Rotate other children (not using BoneAttachment3D due to jitter)
	attitude_root.rotation = target.basis.get_euler()
	
	# Rotate the cyclic stick bone
	var cyclic := connector.get_cyclic()
	var cyc_rest := skeleton.get_bone_rest(bone_cyclic).basis.get_rotation_quaternion()
	var cyc_angles_rad := cyclic * deg_to_rad(range_cyclic)
	var cyc_att := Quaternion.from_euler(Vector3(cyc_angles_rad.y, cyc_angles_rad.x, 0))
	skeleton.set_bone_pose_rotation(bone_cyclic, cyc_att * cyc_rest)
	
	# Spin the rotor only when receiving flight data
	if connector.get_model_connected():
		_rotor_azimuth += rotor_rpm * 2 * PI / 60 * delta
		var rotor_rest := skeleton.get_bone_rest(bone_rotor).basis.get_rotation_quaternion()
		var rotor_att := Quaternion.from_euler(Vector3(0, _rotor_azimuth, 0))
		# Note that this is reverse order, to rotate in local bone axes
		skeleton.set_bone_pose_rotation(bone_rotor, rotor_rest * rotor_att)