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 axis_scene_template: PackedScene = preload("res://axis/Axis.tscn")
var sensor_suites = {}
var sensor_classes = {
	"PNT": [],
	"ACCEL": [],
	"GYRO": [],
	"MAG": []
}
var datetime_sensors = {}
var gps_sensors = {}
var accelerometer_sensors = {}
var gyro_sensors = {}
var magnet_sensors = {}

var current_timestamp = 0.
var lat_rad: float = 0.
var long_rad: float = 0.
var alt_km: float = 0.

var position_3d: Vector3 = Vector3.ZERO

var heading_rad: float = 0.


# Called when the node enters the scene tree for the first time.
func _ready() -> void:
	if host:
		_load_id()
	Signals.position_updated.connect(_on_pnt_position_updated)
	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:
	var time_dict = Time.get_time_dict_from_system(true)
	var unix_time = Time.get_unix_time_from_system()
	current_timestamp = time_dict["hour"] * 3600 + time_dict["minute"] * 60 + time_dict["second"]

func _physics_process(delta: float) -> void:
	pass
	#_update_position()
	#_update_rotation()

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 _update_position():
	var time_offset = 0.
	var sensor_data_array = []
	for sensor_id in gps_sensors:
		var sensor = sensor_suites[sensor_id]
		var sensor_time_offset = Constants.calc_time_diff(sensor.gps_timestamp, current_timestamp)
		time_offset += sensor_time_offset
		sensor_data_array.append([sensor_time_offset, sensor.gps_position])
	var avg_position = Vector3.ZERO
	for sensor_data in sensor_data_array:
		avg_position += (sensor_data[0]/time_offset) * sensor_data[1]
	position = avg_position
	#print(position)
	#print(position.length())
	var camera: Camera3D = get_node("/root/Main/Node3D/Camera3D")
	camera.position = position + position.normalized() * 1
	if camera.position != position:
		camera.look_at(position, Vector3(0, 1, 0))

func _update_rotation():
	if position.length() == 0:
		return
	var down_vec = -1 * position.normalized()
	var up_axis = Vector3(0, 1, 0)
	up_axis = (up_axis * quaternion).normalized()
	var rot_axis = down_vec.cross(up_axis).normalized()
	var rot_deg = up_axis.angle_to(down_vec)
	var rot_quat = Quaternion(rot_axis, rot_deg).normalized()
	quaternion = (quaternion * rot_quat).normalized()

func _on_pnt_position_updated(sensor_ip: String, sensor_position_3d: Vector3):
	if sensor_ip not in sensor_suites:
		return
	if sensor_ip not in sensor_classes["PNT"]:
		return
	position_3d = sensor_position_3d

func add_sensor_node(sensor_node: SensorSuiteNode):
	sensor_suites[sensor_node.ip_address] = sensor_node
	if sensor_node.provides_pnt:
		sensor_classes["PNT"].append(sensor_node.ip_address)
	if sensor_node.provides_accel:
		sensor_classes["ACCEL"].append(sensor_node.ip_address)
	if sensor_node.provides_gyro:
		sensor_classes["GYRO"].append(sensor_node.ip_address)
	if sensor_node.provides_mag:
		sensor_classes["MAG"].append(sensor_node.ip_address)