extends BaseSensorNode
class_name PntSensorNode


var lat_rad: float = 0.
var lon_rad: float = 0.
var alt_km: float = 0.
var speed_kmh: float = 0.
var gps_position = Vector3(1., 1., 1.)
var speed_data: Array = []
var lat_data: Array = []
var lon_data: Array = []
var alt_data: Array = []
var position_data: Array = []
var measurement_times: Array = []


# Called when the node enters the scene tree for the first time.
func _ready() -> void:
	super._ready()
	DataSignals.pnt_data_received.connect(_on_data_received)

func _on_data_received(source_ip: String, timestamp_list: Array, coordinates: Array, pnt_speed_kmh: float, dop: Array):
	if source_ip != ip_address:
		return
	var msg_timestamp = timestamp_list[0] * 3600. + timestamp_list[1] * 60. + timestamp_list[2] + timestamp_list[3]
	var insert_idx = Bisect.bisect(measurement_times, msg_timestamp)
	measurement_times.insert(insert_idx, msg_timestamp)
	
	speed_kmh = pnt_speed_kmh
	speed_data.insert(insert_idx, speed_data)
	lat_rad = deg_to_rad(coordinates[0])
	lat_data.insert(insert_idx, lat_rad)
	lon_rad = deg_to_rad(coordinates[1])
	lon_data.insert(insert_idx, lon_rad)
	alt_km = (coordinates[2] + coordinates[3]) / 1000
	alt_data.insert(insert_idx, alt_km)
	var radius_km = (coordinates[2] + coordinates[3] + Constants.EARTH_RADIUS_M) / 1000.
	var pos_x = radius_km * sin(lon_rad) * cos(lat_rad)
	var pos_z = radius_km * sin(lon_rad) * sin(lat_rad)
	var pos_y = radius_km * cos(lon_rad)
	gps_position = Vector3(pos_x, pos_y, pos_z)
	position_data.insert(insert_idx, gps_position)
	
	Signals.sensor_pnt_updated.emit(ip_address, self)
	Signals.altitude_updated.emit(ip_address, alt_km)
	Signals.position_updated.emit(ip_address, gps_position)
	SensorSignals.speed_updated.emit(ip_address, speed_kmh)
	
	while measurement_times.size() > max_measurement_size:
		measurement_times.pop_back()
		speed_data.pop_back()
		lat_data.pop_back()
		lon_data.pop_back()
		alt_data.pop_back()
		position_data.pop_back()