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 alt_coefficients = []
var altitude_m:
	get:
		return _get_current_altitude()
var speed_data = []
var speed_coefficients = []
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 lateral_g_data = []
var x_coefficients = []
var y_coefficients = []
var lateral_accel:
	get:
		return _get_current_gs()

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


func _ready() -> void:
	super._ready()
	DataSignals.raw_data_received.connect(_on_raw_data_received)
	#DataSignals.sat_data_received.connect(_on_sat_data_received)
	DataSignals.accelerometer_data_received.connect(_on_accel_data_received)
	#DataSignals.gyroscope_data_received.connect(_on_gyro_data_received)

func _get_sensor_class():
	return "VEHICLE"

func _on_raw_data_received(source_ip: String, sensor_type: String, timestamp_list: Array, data: Array):
	if source_ip != ip_address:
		return
	if sensor_type == "GPS":
		var parsed_coordinates = [
			(float(data[0]) / 10.**9) - 90,
			(float(data[1]) / 10.**9) - 180,
			(float(data[2]) / 1000.),
			(float(data[3]) / 100.),
		]
		var speed = float(data[4]) / 1000000.
		var dop = [
			float(data[5]) / 1000.,
			float(data[6]) / 1000.,
			float(data[7]) / 1000.,
		]
		_on_gps_data_received(timestamp_list, parsed_coordinates, speed, dop)
	if sensor_type == "ACCEL":
		var accel = Vector3(
			float(data[0]) / 1000000.,
			float(data[1]) / 1000000.,
			float(data[2]) / 1000000.
		)
		var linear_accel = Vector3(
			float(data[3]) / 1000000.,
			float(data[4]) / 1000000.,
			float(data[5]) / 1000000.
		)
		var g_accel = Vector3(
			float(data[6]) / 1000000.,
			float(data[7]) / 1000000.,
			float(data[8]) / 1000000.
		)
		_on_accel_data_received(timestamp_list, accel, linear_accel, g_accel)
	
func _on_gps_data_received(timestamp_list: Array, new_coordinates: Array, speed: float, dop: Array):
	# coordinates = [lat rad, lon rad, alt m, geoid diff m]
	# dop = [pdop, hdop, vdop]
	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
	if gps_timestamps.size() > 0 and (timestamp - gps_timestamps[-1]) < -1:
		gps_timestamps.clear()
		coordinates.clear()
		gps_positions.clear()
		altitudes.clear()
		speed_data.clear()
	if gps_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(gps_timestamps[-1], timestamp):
		return
	var insert_idx = Bisect.reverse_bisect(gps_timestamps, timestamp)
	var altitude = 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)
	speed_data.insert(insert_idx, speed)
	if gps_timestamps.size() > max_data_length:
		gps_timestamps.pop_back()
		coordinates.pop_back()
		gps_positions.pop_back()
		altitudes.pop_back()
		speed_data.pop_back()
	_update_alt_coefficients()
	_update_speed_coefficients()
	
func _on_sat_data_received(source_ip: String, timestamp: float):
	if source_ip != ip_address:
		return

func _on_accel_data_received(timestamp_list: Array, accel: Vector3, linear_accel: Vector3, g_accel: Vector3):
	var timestamp: float = 3600.*timestamp_list[0] + 60.*timestamp_list[1] + timestamp_list[2] + timestamp_list[3]
	if accel_timestamps.size() > 0 and (timestamp - accel_timestamps[-1]) < -1:
		accel_timestamps.clear()
		linear_vecs.clear()
		g_vecs.clear()
		pitch_data.clear()
		roll_data.clear()
		lateral_g_data.clear()
	if accel_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(accel_timestamps[-1], timestamp):
		return
	var insert_idx = Bisect.reverse_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)
	var lateral_g = Vector2(linear_accel.x, linear_accel.y)
	lateral_g_data.insert(insert_idx, lateral_g.normalized()*lateral_g.length())
	if accel_timestamps.size() > max_data_length:
		accel_timestamps.pop_back()
		linear_vecs.pop_back()
		g_vecs.pop_back()
		pitch_data.pop_back()
		roll_data.pop_back()
		lateral_g_data.pop_back()
	_update_avg_pitch()
	_update_avg_roll()
	_update_g_coefficients()

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() > 0 and (timestamp - mag_timestamps[-1]) < -1:
		mag_timestamps.clear()
		mag_vecs.clear()
		heading_data.clear()
	if mag_timestamps.size() == max_data_length and not TimeHelpers.is_timestamp_newer_24h(mag_timestamps[-1], timestamp):
		return
	var insert_idx = Bisect.reverse_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)
	if new_heading_rad < 0:
		new_heading_rad += 2*PI
	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_back()
		mag_vecs.pop_back()
		heading_data.pop_back()

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

func get_derrivative(dataset: Array) -> Array:
	if dataset.size() < 2:
		return [0, []]
	var deltas = []
	var delta_sum = 0
	for i in range(dataset.size()-1):
		deltas.append(dataset[i+1] - dataset[i])
	return [float(delta_sum) / deltas.size(), deltas]

func get_array_avg(dataset: Array):
	var data_sum = 0
	for i in dataset:
		data_sum += i
	return data_sum / dataset.size()

func get_coefficients(dataset: Array, order: int):
	if order == 0:
		return []
	if dataset.size() <= order:
		order = dataset.size() - 1
	var coefficients = []
	var tmp_dataset = dataset
	var dx_result: Array
	for i in range(order):
		dx_result = get_derrivative(tmp_dataset)
		coefficients.append(dx_result[0])
		tmp_dataset = dx_result[1]
	return coefficients

func _update_alt_coefficients():
	if gps_timestamps.size() < 2:
		return
	alt_coefficients = get_coefficients(altitudes, 2)

func _get_current_altitude():
	if gps_timestamps.size() < 3:
		return null
	var current_timestamp = TimeHelpers.get_current_sensor_time()
	var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[0], current_timestamp))
	var current_value = altitudes[0]
	for idx in range(alt_coefficients.size()):
		current_value += alt_coefficients[idx] * (time_diff ** idx+1)
	return current_value

func _update_speed_coefficients():
	if gps_timestamps.size() < 2:
		return
	speed_coefficients = get_coefficients(speed_data, 2)

func _get_current_speed():
	if gps_timestamps.size() < 3:
		return
	var current_timestamp = TimeHelpers.get_current_sensor_time()
	var time_diff = max(0, TimeHelpers.calc_time_diff_24h(gps_timestamps[-1], current_timestamp))
	var current_value = speed_data[0]
	for idx in range(speed_coefficients.size()):
		current_value += speed_coefficients[idx] * (time_diff ** idx+1)
	return current_value

func _update_g_coefficients():
	if accel_timestamps.size() < 2:
		return
	var x_dataset = []
	var y_dataset = []
	for i in lateral_g_data:
		x_dataset.append(i.x)
		y_dataset.append(i.y)
	x_coefficients = get_coefficients(x_dataset, 3)
	y_coefficients = get_coefficients(y_dataset, 3)

func _get_current_gs():
	if accel_timestamps.size() < 3:
		return
	var current_timestamp = TimeHelpers.get_current_sensor_time()
	var time_diff = max(0, TimeHelpers.calc_time_diff_24h(accel_timestamps[0], current_timestamp))
	var current_x = lateral_g_data[0].x
	var current_y = lateral_g_data[0].y
	for idx in range(speed_coefficients.size()):
		current_x += x_coefficients[idx] * (time_diff ** idx+1)
		current_y += y_coefficients[idx] * (time_diff ** idx+1)
	return Vector2(current_x, current_y)

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(min(mag_timestamps.size(), 2)):
		heading_sum += heading_data[i]
	heading_rad = heading_sum / heading_data.size()