class_name VehicleTelemetry
extends RefCounted

var session_id: StringName
var sequence: int
var world_position_m: Vector3
var world_orientation: Quaternion
var world_linear_velocity_mps: Vector3
var local_angular_velocity_rps: Vector3
var depth_m: float
var longitudinal_effort: float
var steering_effort: float
var vertical_effort: float
var high_thrust_active: bool


func _init(
	p_session_id: StringName,
	p_sequence: int,
	p_world_position_m: Vector3,
	p_world_orientation: Quaternion,
	p_world_linear_velocity_mps: Vector3,
	p_local_angular_velocity_rps: Vector3,
	p_depth_m: float,
	p_longitudinal_effort: float,
	p_steering_effort: float,
	p_vertical_effort: float,
	p_high_thrust_active: bool
) -> void:
	session_id = p_session_id
	sequence = p_sequence
	world_position_m = p_world_position_m
	world_orientation = p_world_orientation
	world_linear_velocity_mps = p_world_linear_velocity_mps
	local_angular_velocity_rps = p_local_angular_velocity_rps
	depth_m = p_depth_m
	longitudinal_effort = p_longitudinal_effort
	steering_effort = p_steering_effort
	vertical_effort = p_vertical_effort
	high_thrust_active = p_high_thrust_active


func validation_error(expected_session_id: StringName, expected_sequence: int) -> String:
	if session_id != expected_session_id:
		return "telemetry session_id does not match"
	if sequence != expected_sequence:
		return "telemetry sequence does not match expected tick"
	if not _vector_is_finite(world_position_m):
		return "telemetry world position is not finite"
	if not _quaternion_is_finite(world_orientation):
		return "telemetry orientation is not finite"
	if not _vector_is_finite(world_linear_velocity_mps):
		return "telemetry linear velocity is not finite"
	if not _vector_is_finite(local_angular_velocity_rps):
		return "telemetry angular velocity is not finite"
	if not is_finite(depth_m) or depth_m < 0.0:
		return "telemetry depth must be finite and non-negative"
	for effort: float in [longitudinal_effort, steering_effort, vertical_effort]:
		if not is_finite(effort) or effort < -1.0 or effort > 1.0:
			return "telemetry efforts must be between -1 and 1"
	return ""


func _vector_is_finite(value: Vector3) -> bool:
	return is_finite(value.x) and is_finite(value.y) and is_finite(value.z)


func _quaternion_is_finite(value: Quaternion) -> bool:
	return (
		is_finite(value.x)
		and is_finite(value.y)
		and is_finite(value.z)
		and is_finite(value.w)
	)