@tool
extends Control
## Telemetry conventions: docs/motion_panel.md.

@export var stale_after_seconds: float = 2.0
@export_range(0.1, 100.0) var acceleration_full_scale: float = 5.0:
	set(value):
		acceleration_full_scale = maxf(value, 0.1)
		_refresh()

var attitude := Vector3.ZERO # roll, pitch, heading in degrees
var momentum := Vector3.ZERO
var acceleration := Vector3.ZERO
var _received_ms: int = -1

var _last_stale := false

func _ready() -> void:
	_refresh()

func set_telemetry(orientation_degrees: Vector3, angular_momentum: Vector3, linear_acceleration: Vector3) -> void:
	if not orientation_degrees.is_finite() or not angular_momentum.is_finite() or not linear_acceleration.is_finite():
		return
	attitude = Vector3(wrapf(orientation_degrees.x, -180, 180), wrapf(orientation_degrees.y, -180, 180), fposmod(orientation_degrees.z, 360))
	momentum = angular_momentum
	acceleration = linear_acceleration
	_received_ms = Time.get_ticks_msec()
	_refresh()

func _process(_delta: float) -> void:
	var stale := _is_stale()
	if stale != _last_stale:
		_refresh()

func _is_stale() -> bool:
	return _received_ms >= 0 and (Time.get_ticks_msec() - _received_ms) / 1000.0 > stale_after_seconds

func _number(value: float, precision: int = 2) -> String:
	return "—" if _received_ms < 0 else ("%+.*f" % [precision, value])

func _refresh() -> void:
	if not is_node_ready():
		return
	_last_stale = _is_stale()
	%Status.text = "STALE" if _last_stale else "NO DATA"
	%StatusBanner.visible = _last_stale or _received_ms < 0
	%Status.visible = true
	%Roll.text = "R " + _number(attitude.x, 1) + "°"
	%Pitch.text = "P " + _number(attitude.y, 1) + "°"
	%Heading.text = "H " + ("%05.1f°" % attitude.z if _received_ms >= 0 else "—")
	var accel_labels := [%AccelX, %AccelY, %AccelZ]
	var momentum_labels := [%MomentumX, %MomentumY, %MomentumZ]
	for axis in range(3):
		accel_labels[axis].text = _number(acceleration[axis])
		momentum_labels[axis].text = _number(momentum[axis])
	%FullScale.text = "±%.1f" % acceleration_full_scale
	%Limit.visible = _received_ms >= 0 and (Vector2(acceleration.x, acceleration.y).length() > acceleration_full_scale or absf(acceleration.z) > acceleration_full_scale)
	for graphic in [%Horizon, %HeadingTape, %AccelerationPlot, %VerticalAcceleration]:
		graphic.attitude = attitude
		graphic.acceleration = acceleration
		graphic.available = _received_ms >= 0
		graphic.full_scale = acceleration_full_scale
		graphic.queue_redraw()