extends Node3D
class_name SatelliteWidget3dNode

var sensor_class: String
var used_sensors: Dictionary = {}

var host_vehicle: VehicleNode
var vehicle_3d_node: Node3D
var vehicle_position: Vector3 = Vector3.ZERO
var sat_scene: PackedScene = preload("res://widgets/satellites/satellite.tscn")
var sat_nodes = {}
var need_sat_update: bool = true
var sat_update_timer: float = 0
var sat_update_rate: float = 60.

var lines = []

var GPS_ALT_KM = 20_200

# Called when the node enters the scene tree for the first time.
func _ready() -> void:
	GPS_ALT_KM += Constants.EARTH_RADIUS_KM
	Signals.vehicle_added.connect(_on_vehicle_added)
	Signals.requested_host_vehicle.emit(self)
	Signals.sat_position_updated.connect(_on_sat_position_updated)
	Signals.position_updated.connect(_on_position_updated)

func _process(delta: float):
	sat_update_timer += delta
	if sat_update_timer >= sat_update_rate:
		sat_update_timer = 0
		need_sat_update = true
	if host_vehicle != null:
		return
	Signals.requested_host_vehicle.emit(self)

func _on_vehicle_added(new_vehicle):
	host_vehicle = new_vehicle

func set_host_vehicle(new_vehicle):
	host_vehicle = new_vehicle
	vehicle_3d_node = Node3D.new()
	add_child(vehicle_3d_node)

func _on_sat_position_updated(sensor_ip: String, sat_position_data: Array):
	if vehicle_3d_node == null:
		return
	if not need_sat_update:
		return
	need_sat_update = false
	var current_sat_ids = []
	for sat_data in sat_position_data:
		var sat_id = sat_data[0]
		var el_rad = deg_to_rad(sat_data[1])
		var az_rad = deg_to_rad(sat_data[2])
		current_sat_ids.append(sat_id)
		if sat_id not in sat_nodes:
			_create_sat_node(sat_id)
		_update_sat_position(sat_id, el_rad, az_rad)
	for old_sat_id in sat_nodes.keys():
		if old_sat_id not in current_sat_ids:
			vehicle_3d_node.remove_child(sat_nodes[old_sat_id])
			sat_nodes[old_sat_id].queue_free()
			sat_nodes.erase(old_sat_id)

func _create_sat_node(sat_id: String):
	var new_sat_node = sat_scene.instantiate()
	sat_nodes[sat_id] = new_sat_node
	vehicle_3d_node.add_child(new_sat_node)

func _update_sat_position(sat_id, el_rad, az_rad):
	# vehicle node is rotated to SEZ.
	# Az is rotated clockwise from north
	# 0 el is horizon. 90 is zenith
	var x = sin(el_rad) * cos(az_rad + PI)
	var y = sin(el_rad) * sin(az_rad + PI)
	var z = cos(el_rad)
	var sat_position = Vector3(x, y, z)
	var global_sat_position = ((vehicle_position.normalized() + sat_position) * GPS_ALT_KM)
	var local_sat_position = global_sat_position - vehicle_position
	sat_nodes[sat_id].position = Vector3(local_sat_position.x, local_sat_position.z, local_sat_position.y)
	
func _on_position_updated(sensor_ip: String, physics_position: Vector3):
	if vehicle_3d_node == null:
		return
	vehicle_position = physics_position
	vehicle_3d_node.position = Vector3(physics_position.x, physics_position.z, physics_position.y)