extends BaseSensorNode
class_name VehicleSensorNode

"""
Vehicle sensor can provide the following data fields:
	- GPS
	- Contributing satellites
	- Accelerometer
	- Magnetometer
	- Gyroscope
"""


var max_data_length: int = 10
var gps_timestamps = []
var coordinates = []
var gps_positions = []
var altitudes = []
var altitude_m:
	get:
		return _get_current_altitude()
var speed_data = []
var speed_kmh:
	get:
		return _get_current_speed()

static var DOWN = Vector2(0, -1.)
var accel_timestamps = []
var g_vecs = []
var linear_vecs = []
var pitch_data = []
var pitch_rad: float
var roll_data = []
var roll_rad: float

var mag_timestamps = []
var mag_vecs = []
var heading_data = []
var heading_rad: float


func _ready() -> void:
	DataSignals.gps_data_received.connect(_on_gps_data_received)
	#DataSignals.sat_data_received.connect(_on_sat_data_received)
	DataSignals.accelerometer_data_received.connect(_on_accel_data_received)
	DataSignals.magnetometer_data_received.connect(_on_mag_data_received)
	#DataSignals.gyroscope_data_received.connect(_on_gyro_data_received)

func _get_sensor_class():
	return "VEHICLE"

func _on_gps_data_received(source_ip: String, timestamp_list: Array, new_coordinates: Array, speed_kmh: float, dop: Array):
	# coordinates = [lat rad, lon rad, alt m, geoid diff m]
	# dop = [pdop, hdop, vdop]
	if source_ip != ip_address:
		return
	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
	if gps_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(gps_timestamps[-1], timestamp):
		return
	var insert_idx = Bisect.bisect(gps_timestamps, timestamp)
	var altitude_m = new_coordinates[2] + new_coordinates[3]
	var radius_km = (new_coordinates[2] + new_coordinates[3] + Constants.EARTH_RADIUS_M) / 1000.
	var pos_x = radius_km * sin(new_coordinates[1]) * cos(new_coordinates[0])
	var pos_z = radius_km * sin(new_coordinates[1]) * sin(new_coordinates[0])
	var pos_y = radius_km * cos(new_coordinates[1])
	gps_timestamps.insert(insert_idx, timestamp)
	coordinates.insert(insert_idx, [new_coordinates[0], new_coordinates[1]])
	gps_positions.insert(insert_idx, Vector3(pos_x, pos_y, pos_z))
	altitudes.insert(insert_idx, altitude_m)
	speed_data.insert(insert_idx, speed_kmh)
	if gps_timestamps.size() > max_data_length:
		gps_timestamps.pop_front()
		coordinates.pop_front()
		gps_positions.pop_front()
		altitudes.pop_front()
		speed_data.pop_front()
	
func _on_sat_data_received(source_ip: String, timestamp: float):
	if source_ip != ip_address:
		return

func _on_accel_data_received(source_ip: String, timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3):
	if source_ip != ip_address:
		return
	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
	if accel_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(accel_timestamps[-1], timestamp):
		return
	var insert_idx = Bisect.bisect(accel_timestamps, timestamp)
	var x_z = Vector2(g_accel.x, g_accel.z).normalized()
	var y_z = Vector2(g_accel.y, g_accel.z).normalized()
	var pitch = DOWN.angle_to(x_z)
	var roll = y_z.angle_to(DOWN)
	accel_timestamps.insert(insert_idx, timestamp)
	linear_vecs.insert(insert_idx, linear_accel)
	g_vecs.insert(insert_idx, g_accel)
	pitch_data.insert(insert_idx, pitch)
	roll_data.insert(insert_idx, roll)
	if accel_timestamps.size() > max_data_length:
		accel_timestamps.pop_front()
		linear_vecs.pop_front()
		g_vecs.pop_front()
		pitch_data.pop_front()
		roll_data.pop_front()
	_update_avg_pitch()
	_update_avg_roll()

func _on_mag_data_received(source_ip: String, timestamp_list: Array, mag_vec: Vector3):
	if source_ip != ip_address:
		return
	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
	if mag_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(mag_timestamps[-1], timestamp):
		return
	var insert_idx = Bisect.bisect(mag_timestamps, timestamp)
	var mag_2d = Vector2(mag_vec.x, mag_vec.y)
	mag_2d.normalized()
	var new_heading_rad = atan2(-mag_2d.y, mag_2d.x)
	mag_timestamps.insert(insert_idx, timestamp)
	mag_vecs.insert(insert_idx, mag_vec)
	heading_data.insert(insert_idx, new_heading_rad)
	if mag_timestamps.size() > max_data_length:
		mag_timestamps.pop_front()
		mag_vecs.pop_front()
		heading_data.pop_front()
	_update_avg_heading()

func _on_gyro_data_received(source_ip: String, timestamp: float):
	if source_ip != ip_address:
		return

func _get_current_altitude():
	if gps_timestamps.size() < 3:
		return null
	var altitude_dx = []
	var alt_dx_sum = 0
	var current_timestamp = TimeHelpers.get_current_sensor_time()
	for i in range(gps_timestamps.size()-1, 0, -1):
		var alt_dx = altitudes[i] - altitudes[i-1]
		alt_dx_sum += alt_dx
		altitude_dx.append(alt_dx)
	var altitude_dx2 = []
	var alt_dx2_sum = 0
	for i in range(altitude_dx.size()-1):
		var alt_dx2 = altitude_dx[i] - altitude_dx[i+1]
		alt_dx2_sum += alt_dx2
		altitude_dx2.append(alt_dx2)
	var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[-1], current_timestamp))
	var dv = alt_dx2_sum/altitude_dx2.size()
	var dx = alt_dx_sum / altitude_dx.size()
	return altitudes[-1] + ((dx + (dv * time_diff)) * time_diff)

func _get_current_speed():
	if gps_timestamps.size() < 3:
		return null
	var speed_dx = []
	var speed_dx_sum = 0
	var current_timestamp = TimeHelpers.get_current_sensor_time()
	for i in range(gps_timestamps.size()-1, 0, -1):
		var spd_dx = speed_data[i] - speed_data[i-1]
		speed_dx_sum += spd_dx
		speed_dx.append(spd_dx)
	var speed_dx2 = []
	var speed_dx2_sum = 0
	for i in range(speed_dx.size()-1):
		var spd_dx2 = speed_dx[i] - speed_dx[i+1]
		speed_dx2_sum += spd_dx2
		speed_dx2.append(spd_dx2)
	var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[-1], current_timestamp))
	var dv = speed_dx2_sum/speed_dx2.size()
	var dx = speed_dx_sum / speed_dx.size()
	return speed_data[-1] + ((dx + (dv * time_diff)) * time_diff)

func _update_avg_pitch():
	var pitch_sum = 0
	for i in range(accel_timestamps.size()):
		pitch_sum += pitch_data[i]
	pitch_rad = pitch_sum / pitch_data.size()

func _update_avg_roll():
	var roll_sum = 0
	for i in range(accel_timestamps.size()):
		roll_sum += roll_data[i]
	roll_rad = roll_sum / roll_data.size()

func _update_avg_heading():
	var heading_sum = 0
	for i in range(mag_timestamps.size()):
		heading_sum += heading_data[i]
	heading_rad = heading_sum / heading_data.size()