extends Node3D
class_name VehicleNode

var UUID = preload('res://addons/uuid/uuid.gd')

"""
	4      | 1 byte | bool | peer provides datetime
	5      | 1 byte | bool | peer provides gps
	6      | 1 byte | bool | peer provides accelerometer
	7     | 1 byte | bool | peer provides gyroscope
	8     | 1 byte | bool | peer provides magnetometer
"""

var host_id: String = ''
@export var host: bool = true
var sensor_nodes = {}
var sensor_classes = {
	"VEHICLE": [],
	"LIDAR_2D": [],
}

var timer: float = 0.
static var timer_1: float = 1.

var altitude_m: float:
	get:
		return _get_altitude_m()

var speed_kmh: float:
	get:
		return _get_speed_kmh()

var roll_rad: float:
	get:
		return _get_roll_rad()

var pitch_rad: float:
	get:
		return _get_pitch_rad()

var lateral_accel: Vector2:
	get:
		return _get_lateral_accel()

var heading_rad: float:
	get:
		return _get_heading_rad()


# Called when the node enters the scene tree for the first time.
func _ready() -> void:
	if host:
		_load_id()
	print('VehicleNode ready')
	#var axis_scene = axis_scene_template.instantiate()
	#axis_scene.axis_length = 2.
	#if not has_node('Axis'):
	#	add_child(axis_scene)

# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta: float) -> void:
	pass

func _physics_process(delta: float) -> void:
	pass

func _load_id():
	var file_path = "user://host_id.idf"
	if not FileAccess.file_exists(file_path):
		host_id = _generate_id()
		var id_file = FileAccess.open(file_path, FileAccess.WRITE)
		id_file.store_line(host_id)
		id_file.close()
	var id_file = FileAccess.open(file_path, FileAccess.READ)
	host_id = id_file.get_line()
	
func _generate_id():
	return UUID.new().as_string()
	
func _get_altitude_m():
	var alt_sum: float = 0
	var alt_count: int = 0
	for sensor_ip in sensor_classes["VEHICLE"]:
		var sensor_alt = sensor_nodes[sensor_ip].altitude_m
		if sensor_alt == null:
			continue
		alt_sum += sensor_alt
		alt_count += 1
	if alt_count == 0:
		return 0
	return alt_sum / alt_count

func _get_speed_kmh():
	var speed_sum: float = 0
	var speed_count: int = 0
	for sensor_ip in sensor_classes["VEHICLE"]:
		var sensor_speed = sensor_nodes[sensor_ip].speed_kmh
		if sensor_speed == null:
			continue
		speed_sum += sensor_speed
		speed_count += 1
	if speed_count == 0:
		return 0
	return speed_sum / speed_count
	
func _get_roll_rad():
	var roll_sum: float = 0
	var roll_count: int = 0
	for sensor_ip in sensor_classes["VEHICLE"]:
		var sensor_roll = sensor_nodes[sensor_ip].roll_rad
		if sensor_roll == null:
			continue
		roll_sum += sensor_roll
		roll_count += 1
	if roll_count == 0:
		return 0
	return roll_sum / roll_count
	
func _get_pitch_rad():
	var pitch_sum: float = 0
	var pitch_count: int = 0
	for sensor_ip in sensor_classes["VEHICLE"]:
		var sensor_pitch= sensor_nodes[sensor_ip].pitch_rad
		if sensor_pitch == null:
			continue
		pitch_sum += sensor_pitch
		pitch_count += 1
	if pitch_count == 0:
		return 0
	return pitch_sum / pitch_count

func _get_lateral_accel():
	var gs_sum: Vector2 = Vector2.ZERO
	var gs_count: int = 0
	for sensor_ip in sensor_classes["VEHICLE"]:
		var sensor_gs = sensor_nodes[sensor_ip].lateral_accel
		if sensor_gs == null:
			continue
		gs_sum += sensor_gs
		gs_count += 1
	if gs_count == 0:
		return Vector2.ZERO
	return gs_sum / gs_count

func _get_heading_rad():
	var heading_sum: float = 0
	var heading_count: int = 0
	for sensor_ip in sensor_classes["VEHICLE"]:
		var sensor_heading = sensor_nodes[sensor_ip].heading_rad
		if sensor_heading == null:
			continue
		heading_sum += sensor_heading
		heading_count += 1
	if heading_count == 0:
		return 0
	return heading_sum / heading_count

func add_sensor_node(sensor_node: BaseSensorNode):
	sensor_nodes[sensor_node.ip_address] = sensor_node
	if sensor_node.sensor_class not in sensor_classes:
		return
	sensor_classes[sensor_node.sensor_class].append(sensor_node.ip_address)