extends SubViewportContainer
class_name OrientationGizmo

const MIN_ACCEL_LEN := 1000.0
const SMOOTH := 0.25
const VIEW_SIZE := Vector2(220, 220)
const ACTIVE_MODULATE := Color(1, 1, 1, 1)
const IDLE_MODULATE := Color(0.55, 0.58, 0.65, 0.85)

## Mesh TOP = local −Y.
## Flat   accel ≈ +Z  → side view, TOP at top of screen
## Facing accel ≈ −Y  → TOP face toward camera
## Steer  accel ≈ −X  → facing + roll left (steering wheel)
var _basis_flat: Basis = Basis(Quaternion(Vector3.RIGHT, PI))
var _basis_facing: Basis = Basis(Quaternion(Vector3.RIGHT, -PI * 0.5))

@onready var _viewport: SubViewport = $SubViewport
@onready var _camera: Camera3D = $SubViewport/Camera3D
@onready var _pod: Node3D = $SubViewport/Pod

var _has_sample := false
var _target_basis := Basis.IDENTITY


func _ready() -> void:
	custom_minimum_size = VIEW_SIZE
	stretch = true
	_viewport.own_world_3d = true
	_viewport.transparent_bg = true
	_viewport.size = Vector2i(VIEW_SIZE)
	_viewport.render_target_update_mode = SubViewport.UPDATE_ALWAYS
	_camera.projection = Camera3D.PROJECTION_ORTHOGONAL
	_camera.size = 2.6
	_camera.near = 0.05
	_camera.far = 50.0
	_camera.position = Vector3(0.0, 0.0, 3.2)
	_camera.look_at(Vector3.ZERO, Vector3.UP)
	clear()


func set_accel(v: Vector3) -> void:
	if v.length() < MIN_ACCEL_LEN:
		return
	_has_sample = true
	modulate = ACTIVE_MODULATE
	_target_basis = _basis_from_accel(v)


func clear() -> void:
	_has_sample = false
	_target_basis = _basis_flat
	if _pod != null:
		_pod.basis = _basis_flat
	modulate = IDLE_MODULATE


func _process(_delta: float) -> void:
	if not _has_sample:
		return
	_pod.basis = _pod.basis.slerp(_target_basis, SMOOTH)


func _basis_from_accel(accel: Vector3) -> Basis:
	var n := accel.normalized()
	# 0 = liegend (+Z), 1 = aufgerichtet / gedreht (wenig +Z)
	var face_t := clampf(1.0 - maxf(n.z, 0.0), 0.0, 1.0)
	var b := _basis_flat.slerp(_basis_facing, face_t)
	# Lenkrad-Roll in der Sensor-XY-Ebene:
	# facing (0,−1) → 0; links (−1,0) → +π/2; rechts (+1,0) → −π/2
	var roll := atan2(-n.x, -n.y)
	return Basis(Quaternion(Vector3.BACK, roll * face_t)) * b