#@tool
extends Node3D

# 模拟对象分类标签的常量
enum  label  {
	UNKNOWN,
	CAR,
	TRUCK,
	BUS,
	TRAILER,
	MOTORCYCLE,
	BICYCLE,
	PEDESTRIAN
}


# 定义不同类型对象的模型池，包含预加载的场景资源和对象池
var model_types = {
	"car": {"scene": preload("res://Dynamic_Object/car.tscn"), "pool": []},
	"pedestrian": {"scene": preload("res://Dynamic_Object/pedestrian.tscn"), "pool": []},
	"truck": {"scene": preload("res://Dynamic_Object/suv.tscn"), "pool": []},
	"trailer": {"scene": preload("res://Dynamic_Object/trailer.tscn"), "pool": []},
	"bus": {"scene": preload("res://Dynamic_Object/bus.tscn"), "pool": []},
	"bicycle": {"scene": preload("res://Dynamic_Object/bicycle.tscn"), "pool": []},
	"motorcycle": {"scene": preload("res://Dynamic_Object/motorcycle.tscn"), "pool": []},
}
func _ready() -> void:

	Websocket.connected_to_server.connect(Callable(self, "connected_to_server"))
	Websocket.message_received.connect(Callable(self, "message_received"))


func connected_to_server():
	update_vector_map_marker()
	update_dynamic_object_marker()
	update_path_drivable_area_marker()
	update_trajectory_marker()
	update_global_path_traiectory()
	
	#%LoadingPage.set_jd(50)
	pass # Replace with function body

func message_received(data: Dictionary):	
	if data.is_empty():
		return
	if !data.has("topic"):return
	match data.topic:
		"/map/vector_map_marker":
			var vector_map_list = []
			var vector_map_list_ns = []
			for pos in data.msg.markers:
				for marker in pos.points:	# visualisation all
					vector_map_list.append(Vector3(marker.x, marker.z, -marker.y))
				
				visualize_mesh(vector_map_list)
				
				# visualisation parking space chaging color 
				if pos.ns == "parking_space":
					#print("Detected parking space marker with id: ", pos.id)
					var a = func ():
						# parking space 的坐标
						for node in %ParkingSpace.get_children():
							node.queue_free()
						var verts = []
						for i in pos.points.size()/6:
							vector_map_list_ns = []
							
							for index in 6:
								var _marker = pos.points[(i*6)+index]
								#print((i*6)+index)
								vector_map_list_ns.append(Vector3(_marker.x, _marker.z, -_marker.y))
								
								verts.append(Vector3(_marker.x, _marker.z, -_marker.y))
							
							visualize_mesh_ps(vector_map_list_ns)
							#await  get_tree().create_timer(2).timeout
						#$StaticBody3D/CollisionShape3D.shape = ConcavePolygonShape3D.new()
						#$StaticBody3D/CollisionShape3D.shape.set_faces(verts)
					a.call()
							
			#%LoadingPage.set_jd(100)
			

		"/perception/object_recognition/objects":
			var objects_list = {}
			for obj in data.msg.objects:
				if obj.classification[0].label == label.UNKNOWN:
					continue
				
				var pos = obj.kinematics.initial_pose_with_covariance.pose.position
				var quat = obj.kinematics.initial_pose_with_covariance.pose.orientation
				var shape = obj.shape
				
				# 将字典转换为 Quaternion 对象
				var my_quat = dictionary_to_quaternion(quat)
				
				var euler_angles = quat_to_euler(my_quat)
				var roll = euler_angles.x
				var pitch = euler_angles.y
				var yaw = euler_angles.z
				
				#print("test yaw data: ", yaw)

				var dynamic_object = {
					"position": ros2_to_godot(pos.x, pos.y, pos.z),
					
					"rotation": ros2_to_godot(roll, pitch, yaw),
					"size": ros2_to_godot(shape.dimensions.x, shape.dimensions.y, shape.dimensions.z)
				}

				var calss_name = ret_class(obj.classification[0].label)
				if !objects_list.has(calss_name):
					objects_list[calss_name] = []
				objects_list[calss_name].append(dynamic_object)
				
			visualize_dynamic_objects(objects_list)
			
		# path drivable area marker
		"/planning/scenario_planning/lane_driving/behavior_planning/path":
			var arr = []
			var arr2 = []
			for post in data.msg.left_bound:
				arr.append(Vector3(post.x,post.z,-post.y))
			for post in data.msg.right_bound:
				arr2.append(Vector3(post.x,post.z,-post.y))
			add_ptah(arr2,%left_line)
			add_ptah(arr,%right_line)
			
			
			var vertices = []
			for point in data.msg.points:
				var post = (point.pose.position)
				vertices.push_back(Vector3(post.x, post.z, -post.y))
				pass
			%GlobalPathTraiectory.add_ptah(vertices,%GlobalPathTraiectory,preload("res://line_4.tres"))

		"/planning/scenario_planning/trajectory":
			var vertices = []
			for point in data.msg.points:
				var post = (point.pose.position)
				vertices.push_back(Vector3(post.x, post.z, -post.y))
				pass
			$Traiectory.add_ptah(vertices,$Traiectory)

		"/planning/path_candidate/start_planner":
			pass
			
			
			

func ret_class(id):
	match id:
				label.PEDESTRIAN:
					return"pedestrian"
				label.BICYCLE:
					return"bicycle"
				label.CAR:
					return"car"
				label.TRUCK:
					return"truck"
				label.MOTORCYCLE:
					return"motorcycle"
				label.BUS:
					return"bus"
				label.TRAILER:
					return"trailer"
				_:
					return"unknown"
# 将字典转换为 Quaternion
func dictionary_to_quaternion(quat_dict: Dictionary) -> Quaternion:
	return Quaternion(quat_dict["x"], quat_dict["y"], quat_dict["z"], quat_dict["w"])


# 将四元数转换为欧拉角的函数
func quat_to_euler(quat: Quaternion) -> Vector3:
	var basis = Basis(quat)  # 使用四元数创建一个 Basis 对象
	var euler = basis.get_euler()  # 获取欧拉角，返回一个包含滚转、俯仰和偏航角的 Vector3
	return euler

# parking space 可视化函数
func visualize_mesh_ps(parking_space_list):
	var st = StaticBody3D.new()
	%ParkingSpace.add_child(st)
	var co = CollisionShape3D.new()
	st.add_child(co)
	co.shape = ConcavePolygonShape3D.new()
	co.shape.set_faces(parking_space_list)
	
	var node = MeshInstance3D.new()
	st.add_child(node)
	node.name = "mesh"
	
	
	node.mesh = ArrayMesh.new()
	
	var arr = []
	arr.resize(Mesh.ARRAY_MAX)
	var verts = PackedVector3Array()
	var normals = PackedVector3Array()

	for point in parking_space_list:
		verts.append(point)
		normals.append(Vector3(0, 1, 0))
	arr[Mesh.ARRAY_VERTEX] = verts
	arr[Mesh.ARRAY_NORMAL] = normals
	node.mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arr)
	
	node.mesh.surface_set_material(0,preload("res://ParkingSpace.tres"))  # parking space color

# 可视化路面的函数
func visualize_mesh(triangle_list):
	$RoadSurface.mesh = ArrayMesh.new()
	
	var arr = []
	arr.resize(Mesh.ARRAY_MAX)
	var verts = PackedVector3Array()
	var normals = PackedVector3Array()

	for point in triangle_list:
		verts.append(point)
		normals.append(Vector3(0, 1, 0))
	arr[Mesh.ARRAY_VERTEX] = verts
	arr[Mesh.ARRAY_NORMAL] = normals
	$RoadSurface.mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arr)
	$RoadSurface.mesh.surface_set_material(0,preload("res://line_3.tres"))

func visualize_dynamic_objects(objects_list):
	
	for key in objects_list.keys():
		
		var cha_zhi = model_types[key].pool.size() - objects_list[key].size()
		if cha_zhi <= 0:
			for i in abs(cha_zhi):
				var node = model_types[key].scene.instantiate()
				model_types[key].pool.append(node)
				node.visible = false
				$DynamicObject.add_child(node)
		for node in model_types[key].pool:
			node.visible = false
		for index in objects_list[key].size():
			var obj = objects_list[key][index]
			var node = model_types[key].pool[index]
			node.visible = true
			#node.position = obj.position
			#node.rotation = obj.rotation
			#node.scale = obj.size
			
			node.mb_post = (Vector3(obj["position"].x, obj["position"].y - obj["size"].y * 0.5, obj["position"].z))
			node.mb_rotation = (obj["rotation"])
			
	pass

func add_ptah(arr,node):
		#print(arr)
		var vertices = PackedVector3Array()
		for index in arr.size():
			if arr.size() <=2:
				if index >= arr.size() -2: break
				var a_post:Vector3 = arr[index]
				var b_post:Vector3 = arr[index + 1]
				var a1 = calculate_A2_position(Vector2(a_post.x,a_post.z),Vector2(b_post.x,b_post.z),0.1)
				
				vertices.push_back(a_post)
				vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
				vertices.push_back(b_post)
				
				var b1 = calculate_A2_position(Vector2(b_post.x,b_post.z),Vector2(a_post.x,a_post.z),0.1)
				
				vertices.push_back(Vector3(b1.x,b_post.y,b1.y))
				vertices.push_back(b_post)
				
				vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
			else:
				if index >= arr.size() -2: break
				var a_post:Vector3 = arr[index]
				var b_post:Vector3 = arr[index + 1]
				var c_post:Vector3 = arr[index + 2]
				var a1 = calculate_A2_position(Vector2(a_post.x,a_post.z),Vector2(b_post.x,b_post.z),0.1)
				
				vertices.push_back(a_post)
				vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
				vertices.push_back(b_post)
				
				var b1 = calculate_A2_position(Vector2(b_post.x,b_post.z),Vector2(c_post.x,c_post.z),0.1)
				
				vertices.push_back(Vector3(b1.x,b_post.y,b1.y))
				vertices.push_back(b_post)
				
				vertices.push_back(Vector3(a1.x,a_post.y,a1.y))
		# 初始化 ArrayMesh。
		var arr_mesh = ArrayMesh.new()
		var arrays = []
		arrays.resize(Mesh.ARRAY_MAX)
		arrays[Mesh.ARRAY_VERTEX] = vertices
		# 创建 Mesh。
		arr_mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arrays)
		node.mesh = arr_mesh
		node.mesh.surface_set_material(0,preload("res://line.tres"))
		pass
		
func calculate_A2_position(A1: Vector2, B1: Vector2, distance_to_A2: float) -> Vector2:
	var m_l2 = (B1.y - A1.y) / (B1.x - A1.x)	# 计算m_l2 (B1 和 A1之间的斜率)
	var m_l1 = -1 / m_l2	# 计算m_l1 (垂直线的斜率)
	var dx = distance_to_A2 / sqrt(1 + pow(m_l1, 2))	# 计算 dx 和 dy
	var dy = m_l1 * dx
	# 计算 A2 的位置 (有两个可能的解)
	var A2_1 = Vector2(A1.x + dx, A1.y + dy)
	var A2_2 = Vector2(A1.x - dx, A1.y - dy)
	
	var direction_to_b1 = (B1 - A1).normalized()
	if direction_to_b1.cross(A2_1 - A1) > 0:
		return A2_1
	else:
		return A2_2

# 可视化矢量地图
func update_vector_map_marker():  
	var send_data = {
		"op": "subscribe",
		"topic": "/map/vector_map_marker",
		"type": "visualization_msgs/msg/MarkerArray"
	}
	Websocket.send_msg(var_to_str(send_data))

# 可视化障碍物
func update_dynamic_object_marker():  
	var send_data = {
		"op": "subscribe",
		"topic": "/perception/object_recognition/objects",
		"type": "autoware_auto_perception_msgs/msg/PredictedObjects"
	}

	Websocket.send_msg(var_to_str(send_data))


# path drivable area marker
func update_path_drivable_area_marker():
	var send_data = {
		"op": "subscribe",
		"topic": "/planning/scenario_planning/lane_driving/behavior_planning/path",
		"type": "autoware_auto_planning_msgs/msg/Path"
	}

	Websocket.send_msg(var_to_str(send_data))


# trajectory marker
func update_trajectory_marker():
	var send_data = {
		"op": "subscribe",
		"topic": "/planning/scenario_planning/trajectory",
		"type": "autoware_auto_planning_msgs/msg/Trajectory"
	}

	Websocket.send_msg(var_to_str(send_data))

# add ptah left_line and right_line
func update_global_path_traiectory():
	var send_data = {
		"op": "subscribe",
		"topic": "/planning/scenario_planning/lane_driving/behavior_planning/path",
		"type": "autoware_auto_planning_msgs/msg/Path"
	}

	Websocket.send_msg(var_to_str(send_data))


# ROS 到 Godot 的转换函数
func ros2_to_godot(x, y, z) -> Vector3:
	# 将 ROS 的坐标 (x, y, z) 转换为 Godot 的坐标 (x, z, -y)
	return Vector3(x, z, -y)