extends Node3D
## Main entry point for G1 Teleop Quest 3 app.
## Initializes XR session with passthrough, wires body tracker to WebSocket client.

@onready var body_tracker: Node = $BodyTracker
@onready var teleop_client: Node = $TeleopClient
@onready var xr_origin: XROrigin3D = $XROrigin3D
@onready var xr_camera: XRCamera3D = $XROrigin3D/XRCamera3D
@onready var webcam_quad: MeshInstance3D = $XROrigin3D/XRCamera3D/WebcamQuad

var xr_interface: XRInterface
var xr_is_focused: bool = false


func _ready() -> void:
	# Initialize OpenXR interface
	xr_interface = XRServer.find_interface("OpenXR")
	if xr_interface and xr_interface.is_initialized():
		print("[Main] OpenXR already initialized")
		_on_openxr_ready()
	elif xr_interface:
		xr_interface.connect("session_begun", _on_openxr_session_begun)
		xr_interface.connect("session_focussed", _on_openxr_focused)
		xr_interface.connect("session_stopping", _on_openxr_stopping)
		if not xr_interface.initialize():
			printerr("[Main] Failed to initialize OpenXR")
			get_tree().quit()
			return
		print("[Main] OpenXR initialized, waiting for session")
	else:
		printerr("[Main] OpenXR interface not found. Is the plugin enabled?")
		get_tree().quit()
		return

	# Enable XR on the viewport
	get_viewport().use_xr = true

	# Wire body tracker output to teleop client
	body_tracker.tracking_data_ready.connect(teleop_client._on_tracking_data)

	# Wire webcam frames from teleop client to webcam display
	teleop_client.webcam_frame_received.connect(webcam_quad._on_webcam_frame)


func _on_openxr_session_begun() -> void:
	print("[Main] OpenXR session begun")
	_on_openxr_ready()


func _on_openxr_ready() -> void:
	# Enable passthrough (Quest 3 mixed reality)
	_enable_passthrough()


func _on_openxr_focused() -> void:
	xr_is_focused = true
	print("[Main] OpenXR session focused")


func _on_openxr_stopping() -> void:
	xr_is_focused = false
	print("[Main] OpenXR session stopping")


func _enable_passthrough() -> void:
	# Request passthrough blend mode for mixed reality
	# Environment blend mode 3 = alpha blend (passthrough)
	var openxr = xr_interface as OpenXRInterface
	if openxr:
		# Try to start passthrough
		var modes = openxr.get_supported_environment_blend_modes()
		print("[Main] Supported blend modes: ", modes)
		# XR_ENVIRONMENT_BLEND_MODE_ALPHA_BLEND = 2 in Godot's enum
		if XRInterface.XR_ENV_BLEND_MODE_ALPHA_BLEND in modes:
			openxr.set_environment_blend_mode(XRInterface.XR_ENV_BLEND_MODE_ALPHA_BLEND)
			print("[Main] Passthrough enabled (alpha blend)")
			# Clear background to transparent
			get_viewport().transparent_bg = true
			RenderingServer.set_default_clear_color(Color(0, 0, 0, 0))
		else:
			print("[Main] Alpha blend not supported, using opaque mode")