extends Node2D

var line: Line2D

var points_angle = []
var points_distance = []
var timer_static = 60
var timer = 0.

# Called when the node enters the scene tree for the first time.
func _ready() -> void:
	line = get_node('Line2D')
	DataSignals.lidar_2d_data_received.connect(_on_lidar_data_received)


# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta: float) -> void:
	timer += delta
	if timer > timer_static:
		timer = 0
		points_angle.clear()
		points_distance.clear()
		for i in line.points:
			print(i - Vector2(400, 400))
		print()
	_update_line()

func _on_lidar_data_received(sensor_ip: String, timestamp_list: Array, lidar_point_count: int, lidar_data: Array):
	for point in lidar_data:
		var insert_idx = Bisect.bisect(points_angle, point[0])
		points_angle.insert(insert_idx, point[0])
		points_distance.insert(insert_idx, point[1])

func _update_line():
	line.clear_points()
	for idx in range(points_angle.size()):
		var x = 4*points_distance[idx] * cos(deg_to_rad(points_angle[idx])) + 400
		var y = 4*points_distance[idx] * sin(deg_to_rad(points_angle[idx])) + 400
		line.add_point(Vector2(x, y))