extends Node3D

signal use_fallback(active: bool)
signal fog_density_changed(new_density: float)

## 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
@export var track_platform: bool = false
@export var fog_values: Array[float] = [0.0, -1.0, -2.0, -3.0, -4.0]

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

@onready var connector: MarshConnector = $MarshConnector
@onready var skeleton: Skeleton3D = $"Mi-2/Armature/Skeleton3D"
@onready var attitude_root: Node3D = $AttitudeRoot
@onready var tracker_mount_chair: Node3D = $AttitudeRoot/PilotEyes/PilotFloor/TrackerMountChair
@onready var tracker_mount_platform: Node3D = $AttitudeRoot/PilotEyes/PilotFloor/TrackerMountPlatform
@onready var xr_origin: XROrigin3D = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D
@onready var tracker: XRController3D = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D/ViveTracker
@onready var xr_camera: XRCamera3D = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D/XRCamera3D
@onready var recent_tracking: Timer = $AttitudeRoot/PilotEyes/PilotFloor/XROrigin3D/RecentTracking

@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")

@onready var instruments: Node = $AttitudeRoot/Instruments

func _process(delta: float) -> void:
	var target: Transform3D = connector.get_aircraft()
	position = target.origin

	# Add the rotation to the correct bone for rotation around CG
	var rest: Quaternion = skeleton.get_bone_rest(bone_cg).basis.get_rotation_quaternion()
	var attitude: Quaternion = 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: Vector2 = connector.get_cyclic()
	var cyc_rest: Quaternion= skeleton.get_bone_rest(bone_cyclic).basis.get_rotation_quaternion()
	var cyc_angles_rad: Vector2 = 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)
	
	# Update the instruments
	instruments.call("set_controls", connector.get_controls())
	instruments.call("set_trim", connector.get_trim())
	instruments.call("update_pfd", connector.get_aircraft(), connector.get_velocity())

	# Spin the rotor only when receiving flight data
	if connector.get_model_connected():
		_rotor_azimuth += rotor_rpm * 2 * PI / 60 * delta
		var rotor_rest: Quaternion = 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)

	# Move the XR origin in such way that the tracker is in reference position
	if tracker.get_has_tracking_data():
		var tracker_mount: Node3D = tracker_mount_platform if track_platform else tracker_mount_chair
		xr_origin.transform = tracker_mount.transform * tracker.transform.affine_inverse()
		recent_tracking.start()

func _input(event):
	if event.is_action_pressed("fog_cycle"):
		var step = 1
		if event is InputEventWithModifiers and event.shift_pressed:
			step = -1
		# The index is updated in the signal callback to handle parameter changes
		connector.fog_density = fog_values[(fog_index + step) % len(fog_values)]

# Only reset the position when tracking is lost for some time
func _on_recent_tracking_timeout() -> void:
	xr_origin.transform = Transform3D.IDENTITY

func _on_vr_setup_failed() -> void:
	xr_camera.current = false
	use_fallback.emit(true)

func _on_marsh_connector_fog_density_changed(new_density: float) -> void:
	# Forward to parent
	fog_density_changed.emit(new_density)

	# Round down to nearest defined value or assign the last one (so it will disable on next cycle)
	# Example: -1.5 should select -2 and on next cycle it will be -3
	var new_index = fog_values.find_custom(func (d): return d <= new_density)
	if new_index == -1:
		new_index = len(fog_values) - 1
	fog_index = new_index