extends Node3D

@onready var view_camera: Camera3D = %ViewCamera
@onready var side_display_sub_viewport: SubViewport = $SideDisplaySubViewport
@onready var main_sub_viewport: SubViewport = $MainSubViewport
@onready var vertical: Node3D = %Vertical
@onready var camera_3d: Node3D = %Camera3D

func _init() -> void:
	Global.EgoVehicle3D = self
	
func _ready() -> void:
	Websocket.connected_to_server.connect(connected_to_server)
	Websocket.message_received.connect(message_received)
func SideDisplayGetTexture():
	return side_display_sub_viewport.get_texture()
func MainSubGetTexture():
	return main_sub_viewport.get_texture()
func connected_to_server():
	pose()
	pass
var actuation_status = 0
var mb_post = Vector3(0,0,0)
var mb_rotation = Vector3(0,0,0)
func message_received(data: Dictionary):	
	if data.is_empty():
		return
	if !data.has("topic"):return

	match data.topic:
		"/localization/pose_twist_fusion_filter/pose":
			var post = data.msg.pose
			mb_post = Vector3(post.position.x, post.position.z, -post.position.y)
			mb_rotation = quaternion_to_euler(Quaternion(post.orientation.x,post.orientation.y,post.orientation.z,post.orientation.w))
		pass
func _process(delta):
	if mb_post.distance_to(global_position) > 10:
		global_position = mb_post
		rotation = mb_rotation
	else:
		global_position = lerp(global_position,mb_post,1*delta)
		rotation.x = lerp_angle(rotation.x,mb_rotation.x,1*delta)
		rotation.y = lerp_angle(rotation.y,mb_rotation.y,1*delta)
		rotation.z = lerp_angle(rotation.z,mb_rotation.z,1*delta)
	
	#xz(delta*500*actuation_status)
	
	%Camera3D.global_position = global_position
	#%Camera3D.global_position.z = global_position.z
	#%Camera3D.global_position.y = global_position.y 
	%Camera3D.global_rotation_degrees.y = global_rotation_degrees.y - 90
	#$SubViewport2/Camera3D.global_transform = %ViewCamera.global_transform

func pose():#
	var send_data = {
		"op": "subscribe",
		"topic": "/localization/pose_twist_fusion_filter/pose",
		"type": "geometry_msgs/msg/PoseStamped"
	}
	print("post请求发送")
	Websocket.send_msg(str(send_data))
# 将四元数转换为欧拉角（roll, pitch, yaw）
func quaternion_to_euler(quaternion: Quaternion) -> Vector3:
	var roll: float
	var pitch: float
	var yaw: float
	
	# 计算四元数分量的平方
	var sqx = quaternion.x * quaternion.x
	var sqy = quaternion.y * quaternion.y
	var sqz = quaternion.z * quaternion.z
	var sqw = quaternion.w * quaternion.w
	
	# 计算sarg,它代表pitch的sin值
	var sarg = -2 * (quaternion.x * quaternion.z - quaternion.w * quaternion.y)
	var pi_2 = PI/2  # π/2,表示90度的弧度值
	
	# 处理万向节锁情况
	if sarg <= -0.99999:
		pitch = -pi_2  # pitch设为-90度
		roll = 0       # roll设为0
		yaw = 2 * atan2(quaternion.x, -quaternion.y)  # 计算yaw角
	elif sarg >= 0.99999:
		pitch = pi_2  # pitch设为90度
		roll = 0      # roll设为0
		yaw = 2 * atan2(-quaternion.x, quaternion.y)  # 计算yaw角
	else:
		# 在一般情况下,计算pitch, roll和yaw
		pitch = asin(sarg)
		roll = atan2(2 * (quaternion.y * quaternion.z + quaternion.w * quaternion.x), sqw - sqx - sqy + sqz)
		yaw = atan2(2 * (quaternion.x * quaternion.y + quaternion.w * quaternion.z), sqw + sqx - sqy - sqz)
	
	# 返回欧拉角,作为一个Vector3对象
	return Vector3(roll, yaw,  pitch)