import pygame
from .config import FlightStickConfig
from ..xbox import Xbox
"""
FlightStick类用于检测和监控飞行手柄。
"""
class FlightStick(Xbox):
    config_class = FlightStickConfig
    name = "flight_stick"

    def get_action(self):
        pygame.event.pump()
        pose_speeds = [0.0] * 6
        """更新末端位姿控制"""
        # 根据控制模式调整步长
        current_linear_step = self.linear_step * (0.1 if self.fine_control_mode else 1.0)
        current_angular_step = self.angular_step * (0.1 if self.fine_control_mode else 1.0)
        
        # print(f"步长设置 - 线性: {current_linear_step}, 角度: {current_angular_step}")
        print(f"精细控制模式: {self.fine_control_mode}")
        # print(f"{self.joystick.get_axis(0)},{self.joystick.get_axis(1)},{self.joystick.get_axis(2)},{self.joystick.get_axis(3)}")
        # 方向键控制XY
        hat = self.joystick.get_hat(0)
        hat_up = hat[1] == 1     # Y+
        hat_down = hat[1] == -1  # Y-
        hat_left = hat[0] == -1  # X-
        hat_right = hat[0] == 1  # X+
        # 计算各轴速度
        pose_speeds['x.vel'] = -current_linear_step if hat_left else (current_linear_step if hat_right else 0.0)  # X
        pose_speeds['y.vel'] = current_linear_step if hat_up else (-current_linear_step if hat_down else 0.0)  # Y
        
        # print(f"方向键状态: up={hat_up}, down={hat_down}, left={hat_left}, right={hat_right}")
        # 油门控制Z轴
        gas_axis = -self.joystick.get_axis(2)
        
        # 设置Z速度
        z_mapping = self.apply_nonlinear_mapping(gas_axis)
        # print(f"Z轴非线性映射: {gas_axis} -> {z_mapping}")
        pose_speeds['z.vel'] = z_mapping * current_linear_step  # Z

        
        # 主摇杆X轴控制RX旋转
        # 设置RX旋转速度
        rx_mapping = self.apply_nonlinear_mapping(-self.joystick.get_axis(1))
        # print(f"RX轴非线性映射: {x_axis} -> {rx_mapping}")
        pose_speeds['rx.vel'] = rx_mapping * current_angular_step * 2  # RX
        
        # 主摇杆Y轴控制RY旋转
        # 设置RY旋转速度
        ry_mapping = -self.apply_nonlinear_mapping(-self.joystick.get_axis(0))
        # print(f"RY轴非线性映射: {y_axis} -> {ry_mapping}")
        pose_speeds['ry.vel'] = ry_mapping * current_angular_step * 2  # RY
    
        # 主摇杆左右旋转轴 控制RZ旋转
        # 设置RZ旋转速度
        rz_mapping = self.apply_nonlinear_mapping(self.joystick.get_axis(3))
        # print(f"RZ轴非线性映射: {z_axis} -> {rz_mapping}")
        pose_speeds['rz.vel'] = rz_mapping * current_angular_step * 2  # RZ
        
        pose_speeds['gripper.vel'] = 0
        return pose_speeds