g1-teleop/scripts/vr_ui_pointer.gd

extends Node3D
## VR UI pointer with hand tracking visualization, controller ray-pointing,
## and finger poke interaction. Renders hand joints as spheres and shows
## controller placeholder meshes.

@export var ray_length: float = 5.0
@export var poke_threshold: float = 0.03
@export var hover_distance: float = 0.15
@export var laser_color: Color = Color(0.3, 0.6, 1.0, 0.6)

var _xr_origin: XROrigin3D
var _camera: XRCamera3D
var _left_ctrl: XRController3D
var _right_ctrl: XRController3D

var _laser_right: MeshInstance3D
var _laser_left: MeshInstance3D
var _reticle: MeshInstance3D

# Hand joint visualization: _hand_joints[hand_idx][joint_idx] = MeshInstance3D
var _hand_joints: Array = [[], []]
var _ctrl_mesh_left: MeshInstance3D
var _ctrl_mesh_right: MeshInstance3D

var _current_viewport: SubViewport = null
var _last_viewport_pos: Vector2 = Vector2.ZERO
var _is_pressing: bool = false
var _active_method: String = ""
var _log_timer: float = 0.0

const JOINT_COUNT := 26
# Fingertip joint indices for XRHandTracker (thumb=5, index=10, middle=15, ring=20, pinky=25)
const TIP_JOINTS := [5, 10, 15, 20, 25]
const HAND_COLORS := [Color(0.3, 0.6, 1.0, 1.0), Color(0.3, 1.0, 0.6, 1.0)]

# XRBodyTracker fallback: hand joint start indices and count
const BODY_LEFT_HAND_START := 18
const BODY_RIGHT_HAND_START := 43
const BODY_HAND_JOINT_COUNT := 25
# Fingertip indices within 25-joint body tracker hand block
const BODY_TIP_JOINTS := [4, 9, 14, 19, 24]
# Index finger tip within body tracker hand block (for poke interaction)
const BODY_INDEX_TIP := 9


func setup(xr_origin: XROrigin3D, camera: XRCamera3D, left_ctrl: XRController3D, right_ctrl: XRController3D) -> void:
	_xr_origin = xr_origin
	_camera = camera
	_left_ctrl = left_ctrl
	_right_ctrl = right_ctrl

	_left_ctrl.button_pressed.connect(_on_left_button_pressed)
	_left_ctrl.button_released.connect(_on_left_button_released)
	_right_ctrl.button_pressed.connect(_on_right_button_pressed)
	_right_ctrl.button_released.connect(_on_right_button_released)

	# Create lasers (start hidden until controllers are active)
	_laser_right = _create_laser()
	_laser_right.visible = false
	_right_ctrl.add_child(_laser_right)
	_laser_left = _create_laser()
	_laser_left.visible = false
	_left_ctrl.add_child(_laser_left)

	# Reticle dot where pointer intersects UI
	_reticle = MeshInstance3D.new()
	var sphere := SphereMesh.new()
	sphere.radius = 0.01
	sphere.height = 0.02
	_reticle.mesh = sphere
	var rmat := StandardMaterial3D.new()
	rmat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
	rmat.albedo_color = Color(1, 1, 1, 0.9)
	rmat.transparency = BaseMaterial3D.TRANSPARENCY_ALPHA
	rmat.no_depth_test = true
	rmat.render_priority = 10
	_reticle.material_override = rmat
	_reticle.visible = false
	add_child(_reticle)

	# Hand joint spheres
	_create_hand_visuals()

	# Controller placeholder meshes
	_ctrl_mesh_left = _create_controller_mesh(_left_ctrl)
	_ctrl_mesh_right = _create_controller_mesh(_right_ctrl)

	print("[VRUIPointer] Setup complete")


func _create_laser() -> MeshInstance3D:
	var laser := MeshInstance3D.new()
	var cyl := CylinderMesh.new()
	cyl.top_radius = 0.002
	cyl.bottom_radius = 0.002
	cyl.height = ray_length
	laser.mesh = cyl
	laser.position = Vector3(0, 0, -ray_length / 2.0)
	laser.rotation.x = deg_to_rad(90)
	var mat := StandardMaterial3D.new()
	mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
	mat.albedo_color = laser_color
	mat.transparency = BaseMaterial3D.TRANSPARENCY_ALPHA
	laser.material_override = mat
	return laser


func _create_hand_visuals() -> void:
	# Shared meshes - larger for visibility in VR
	var joint_mesh := SphereMesh.new()
	joint_mesh.radius = 0.01
	joint_mesh.height = 0.02
	var tip_mesh := SphereMesh.new()
	tip_mesh.radius = 0.013
	tip_mesh.height = 0.026

	for hand_idx in [0, 1]:
		var mat := StandardMaterial3D.new()
		mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
		mat.albedo_color = HAND_COLORS[hand_idx]

		for joint_idx in range(JOINT_COUNT):
			var s := MeshInstance3D.new()
			s.mesh = tip_mesh if joint_idx in TIP_JOINTS else joint_mesh
			s.material_override = mat
			s.visible = false
			add_child(s)
			_hand_joints[hand_idx].append(s)


func _create_controller_mesh(ctrl: XRController3D) -> MeshInstance3D:
	var mesh_inst := MeshInstance3D.new()
	var box := BoxMesh.new()
	box.size = Vector3(0.05, 0.03, 0.12)
	mesh_inst.mesh = box
	var mat := StandardMaterial3D.new()
	mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
	mat.albedo_color = Color(0.5, 0.5, 0.6, 0.7)
	mat.transparency = BaseMaterial3D.TRANSPARENCY_ALPHA
	mesh_inst.material_override = mat
	mesh_inst.visible = false
	ctrl.add_child(mesh_inst)
	return mesh_inst


func _process(delta: float) -> void:
	if _xr_origin == null:
		return

	# Debug logging every 5 seconds
	_log_timer += delta
	if _log_timer > 5.0:
		_log_timer = 0.0
		var l := _left_ctrl.get_is_active() if _left_ctrl else false
		var r := _right_ctrl.get_is_active() if _right_ctrl else false
		var panels := get_tree().get_nodes_in_group("start_screen").size()
		print("[VRUIPointer] ctrl=L:%s/R:%s panels=%d" % [l, r, panels])
		# List all XR trackers
		var trackers := XRServer.get_trackers(0xFF)
		var tracker_names := []
		for key in trackers:
			tracker_names.append(str(key))
		print("[VRUIPointer] all_trackers=%s" % [", ".join(tracker_names)])
		# Hand tracker diagnostics
		for hand_idx in [0, 1]:
			var side := "L" if hand_idx == 0 else "R"
			var tn := &"/user/hand_tracker/left" if hand_idx == 0 else &"/user/hand_tracker/right"
			var tr = XRServer.get_tracker(tn)
			if tr == null:
				print("[VRUIPointer] hand_%s: tracker=NULL" % side)
			else:
				var ht = tr as XRHandTracker
				if ht:
					var src = ht.hand_tracking_source
					print("[VRUIPointer] hand_%s: has_data=%s source=%d type=%s" % [side, ht.has_tracking_data, src, ht.get_class()])
					# Sample joint data even if has_data=false
					var wrist := ht.get_hand_joint_transform(0)
					var index_tip := ht.get_hand_joint_transform(10)
					print("[VRUIPointer] hand_%s: wrist=%s idx_tip=%s" % [side, wrist.origin, index_tip.origin])
				else:
					print("[VRUIPointer] hand_%s: tracker exists but NOT XRHandTracker, class=%s" % [side, tr.get_class()])
		# Body tracker diagnostics (fallback for hand data)
		var bt = XRServer.get_tracker(&"/user/body_tracker") as XRBodyTracker
		if bt == null:
			print("[VRUIPointer] body_tracker=NULL")
		else:
			var bt_data := bt.get_has_tracking_data()
			var vis_l := 0
			var vis_r := 0
			for s in _hand_joints[0]:
				if s.visible:
					vis_l += 1
			for s in _hand_joints[1]:
				if s.visible:
					vis_r += 1
			if bt_data:
				var lw := bt.get_joint_transform(BODY_LEFT_HAND_START)
				var rw := bt.get_joint_transform(BODY_RIGHT_HAND_START)
				print("[VRUIPointer] body: has_data=true L_wrist=%s R_wrist=%s vis=L:%d/R:%d" % [lw.origin, rw.origin, vis_l, vis_r])
			else:
				print("[VRUIPointer] body: has_data=false vis=L:%d/R:%d" % [vis_l, vis_r])

	# Update visuals every frame
	_update_hand_visuals()

	# Controller visibility and laser defaults
	var l_active := _left_ctrl.get_is_active()
	var r_active := _right_ctrl.get_is_active()
	_ctrl_mesh_left.visible = l_active
	_ctrl_mesh_right.visible = r_active
	_laser_left.visible = l_active
	_laser_right.visible = r_active
	if l_active:
		_reset_laser_length(_laser_left)
	if r_active:
		_reset_laser_length(_laser_right)

	# UI interaction
	var panels := _get_ui_panels()
	if panels.is_empty():
		_reticle.visible = false
		return

	var hit := false

	# 1. Hand tracking poke (priority over controller ray)
	for hand_idx in [0, 1]:
		var tip_pos := _get_fingertip_world_position(hand_idx)
		if tip_pos == Vector3.ZERO:
			continue
		for panel in panels:
			var result := _check_point_against_panel(tip_pos, panel[0], panel[1])
			if result.size() > 0 and abs(result[0]) < hover_distance:
				hit = true
				_reticle.global_position = result[2]
				_reticle.visible = true
				_current_viewport = panel[1]
				_last_viewport_pos = result[1]
				_send_mouse_motion(panel[1], result[1])

				var method := "poke_%d" % hand_idx
				if result[0] < poke_threshold and not _is_pressing:
					_is_pressing = true
					_active_method = method
					_send_mouse_button(panel[1], result[1], true)
				elif result[0] >= poke_threshold and _is_pressing and _active_method == method:
					_is_pressing = false
					_send_mouse_button(panel[1], result[1], false)
				break
		if hit:
			break

	# 2. Controller ray pointing
	if not hit:
		for ctrl_data in [[_right_ctrl, _laser_right, "ray_right"], [_left_ctrl, _laser_left, "ray_left"]]:
			var ctrl: XRController3D = ctrl_data[0]
			var laser: MeshInstance3D = ctrl_data[1]
			var method: String = ctrl_data[2]
			if not ctrl.get_is_active():
				continue
			var ray_origin := ctrl.global_position
			var ray_dir := -ctrl.global_transform.basis.z.normalized()

			for panel in panels:
				var result := _ray_intersect_panel(ray_origin, ray_dir, panel[0], panel[1])
				if result.size() > 0:
					hit = true
					_reticle.global_position = result[2]
					_reticle.visible = true
					_current_viewport = panel[1]
					_last_viewport_pos = result[1]
					_send_mouse_motion(panel[1], result[1])
					var dist := ray_origin.distance_to(result[2])
					_update_laser_length(laser, dist)
					break
			if hit:
				break

	if not hit:
		_reticle.visible = false
		if _is_pressing:
			_is_pressing = false
			if _current_viewport:
				_send_mouse_button(_current_viewport, _last_viewport_pos, false)
		_current_viewport = null


func _has_hand_tracking(hand: int) -> bool:
	var tracker_name := &"/user/hand_tracker/left" if hand == 0 else &"/user/hand_tracker/right"
	var tracker = XRServer.get_tracker(tracker_name) as XRHandTracker
	return tracker != null and tracker.has_tracking_data


func _update_hand_visuals() -> void:
	for hand_idx in [0, 1]:
		# Try XRHandTracker first (works with controllers)
		var tracker_name := &"/user/hand_tracker/left" if hand_idx == 0 else &"/user/hand_tracker/right"
		var tracker = XRServer.get_tracker(tracker_name)
		var hand_tracker: XRHandTracker = tracker as XRHandTracker if tracker else null

		if hand_tracker and hand_tracker.has_tracking_data:
			for joint_idx in range(JOINT_COUNT):
				var xform := hand_tracker.get_hand_joint_transform(joint_idx)
				if xform.origin == Vector3.ZERO:
					_hand_joints[hand_idx][joint_idx].visible = false
					continue
				var world_pos := _xr_origin.global_transform * xform.origin
				_hand_joints[hand_idx][joint_idx].global_position = world_pos
				_hand_joints[hand_idx][joint_idx].visible = true
			continue

		# Fallback: XRBodyTracker (Meta FB body tracking, works without controllers)
		var body_tracker = XRServer.get_tracker(&"/user/body_tracker") as XRBodyTracker
		if body_tracker and body_tracker.get_has_tracking_data():
			var start_idx := BODY_LEFT_HAND_START if hand_idx == 0 else BODY_RIGHT_HAND_START
			for i in range(BODY_HAND_JOINT_COUNT):
				var xform := body_tracker.get_joint_transform(start_idx + i)
				if xform.origin == Vector3.ZERO:
					if i < JOINT_COUNT:
						_hand_joints[hand_idx][i].visible = false
					continue
				var world_pos := _xr_origin.global_transform * xform.origin
				if i < JOINT_COUNT:
					_hand_joints[hand_idx][i].global_position = world_pos
					_hand_joints[hand_idx][i].visible = true
			# Hide the 26th sphere (body tracker has 25 joints, not 26)
			_hand_joints[hand_idx][25].visible = false
			continue

		# No tracking data from either source
		for s in _hand_joints[hand_idx]:
			s.visible = false


func _update_laser_length(laser: MeshInstance3D, length: float) -> void:
	var cyl := laser.mesh as CylinderMesh
	if cyl:
		cyl.height = length
	laser.position = Vector3(0, 0, -length / 2.0)


func _reset_laser_length(laser: MeshInstance3D) -> void:
	var cyl := laser.mesh as CylinderMesh
	if cyl:
		cyl.height = ray_length
	laser.position = Vector3(0, 0, -ray_length / 2.0)


func _get_fingertip_world_position(hand: int) -> Vector3:
	# Try XRHandTracker first
	var tracker_name := &"/user/hand_tracker/left" if hand == 0 else &"/user/hand_tracker/right"
	var tracker = XRServer.get_tracker(tracker_name)
	if tracker:
		var hand_tracker := tracker as XRHandTracker
		if hand_tracker and hand_tracker.has_tracking_data:
			var tip_xform := hand_tracker.get_hand_joint_transform(XRHandTracker.HAND_JOINT_INDEX_FINGER_TIP)
			if tip_xform.origin != Vector3.ZERO:
				return _xr_origin.global_transform * tip_xform.origin

	# Fallback: XRBodyTracker
	var body_tracker = XRServer.get_tracker(&"/user/body_tracker") as XRBodyTracker
	if body_tracker and body_tracker.get_has_tracking_data():
		var start_idx := BODY_LEFT_HAND_START if hand == 0 else BODY_RIGHT_HAND_START
		var tip_xform := body_tracker.get_joint_transform(start_idx + BODY_INDEX_TIP)
		if tip_xform.origin != Vector3.ZERO:
			return _xr_origin.global_transform * tip_xform.origin

	return Vector3.ZERO


func _get_ui_panels() -> Array:
	var results := []
	for screen in get_tree().get_nodes_in_group("start_screen"):
		if not screen.visible:
			continue
		var ui_mesh: MeshInstance3D = screen.get_node_or_null("UIMesh")
		var vp: SubViewport = screen.get_node_or_null("UIMesh/SubViewport")
		if ui_mesh and vp:
			results.append([ui_mesh, vp])
	return results


func _check_point_against_panel(point: Vector3, mesh: MeshInstance3D, vp: SubViewport) -> Array:
	var mx := mesh.global_transform
	var normal := mx.basis.z.normalized()
	var signed_dist := normal.dot(point - mx.origin)
	var projected := point - normal * signed_dist
	var local_hit := mx.affine_inverse() * projected
	var qm := mesh.mesh as QuadMesh
	if qm == null:
		return []
	var qs := qm.size
	if abs(local_hit.x) > qs.x / 2.0 or abs(local_hit.y) > qs.y / 2.0:
		return []
	var u := (local_hit.x / qs.x) + 0.5
	var v := 0.5 - (local_hit.y / qs.y)
	return [signed_dist, Vector2(u * vp.size.x, v * vp.size.y), projected]


func _ray_intersect_panel(ray_origin: Vector3, ray_dir: Vector3, mesh: MeshInstance3D, vp: SubViewport) -> Array:
	var mx := mesh.global_transform
	var normal := mx.basis.z.normalized()
	var denom := normal.dot(ray_dir)
	if abs(denom) < 0.0001:
		return []
	var t := normal.dot(mx.origin - ray_origin) / denom
	if t < 0 or t > ray_length:
		return []
	var hit := ray_origin + ray_dir * t
	var local_hit := mx.affine_inverse() * hit
	var qm := mesh.mesh as QuadMesh
	if qm == null:
		return []
	var qs := qm.size
	if abs(local_hit.x) > qs.x / 2.0 or abs(local_hit.y) > qs.y / 2.0:
		return []
	var u := (local_hit.x / qs.x) + 0.5
	var v := 0.5 - (local_hit.y / qs.y)
	return [0.0, Vector2(u * vp.size.x, v * vp.size.y), hit]


func _on_right_button_pressed(button_name: String) -> void:
	if button_name in ["trigger_click", "ax_button", "primary_click"]:
		if _current_viewport and not _is_pressing:
			_is_pressing = true
			_active_method = "ray_right"
			_send_mouse_button(_current_viewport, _last_viewport_pos, true)


func _on_right_button_released(button_name: String) -> void:
	if button_name in ["trigger_click", "ax_button", "primary_click"]:
		if _is_pressing and _active_method == "ray_right":
			_is_pressing = false
			if _current_viewport:
				_send_mouse_button(_current_viewport, _last_viewport_pos, false)


func _on_left_button_pressed(button_name: String) -> void:
	if button_name in ["trigger_click", "ax_button", "primary_click"]:
		if _current_viewport and not _is_pressing:
			_is_pressing = true
			_active_method = "ray_left"
			_send_mouse_button(_current_viewport, _last_viewport_pos, true)


func _on_left_button_released(button_name: String) -> void:
	if button_name in ["trigger_click", "ax_button", "primary_click"]:
		if _is_pressing and _active_method == "ray_left":
			_is_pressing = false
			if _current_viewport:
				_send_mouse_button(_current_viewport, _last_viewport_pos, false)


func _send_mouse_motion(vp: SubViewport, pos: Vector2) -> void:
	var event := InputEventMouseMotion.new()
	event.position = pos
	event.global_position = pos
	vp.push_input(event)


func _send_mouse_button(vp: SubViewport, pos: Vector2, pressed: bool) -> void:
	var event := InputEventMouseButton.new()
	event.position = pos
	event.global_position = pos
	event.button_index = MOUSE_BUTTON_LEFT
	event.pressed = pressed
	if pressed:
		event.button_mask = MOUSE_BUTTON_MASK_LEFT
	vp.push_input(event)