#!/usr/bin/env python3
import threading
import rclpy
import os  # 全局导入os模块
from rclpy.node import Node
import evdev
from evdev import ecodes, list_devices, InputDevice, categorize
from interface.msg import CTRL
import fcntl  # 用于文件锁操作


TARGET_VENDOR = 0x1915  # 1915
TARGET_PRODUCT = 0x1025  # 1025


class RemoteControlNode(Node):
    def __init__(self):
        super().__init__("remote_control_node")

        # 查找指定的遥控器键盘设备
        device_path = self.find_target_device()
        if not device_path:
            self.get_logger().error("未找到目标遥控器设备，节点退出")
            raise RuntimeError("找不到可用的遥控器设备")

        # 打开设备并设置非阻塞模式
        self.dev = InputDevice(device_path)
        self._set_non_blocking(self.dev.fd)

        try:
            # 双重保险：先尝试evdev的grab，再用fcntl加锁
            self.dev.grab()
            fcntl.flock(self.dev.fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
            self.get_logger().info(
                f"成功独占设备: {self.dev.name} ({device_path})，所有按键已被拦截"
            )
        except BlockingIOError:
            self.get_logger().error(f"设备 {device_path} 已被其他进程占用，无法独占")
            raise
        except Exception as e:
            self.get_logger().error(f"无法独占设备 {device_path}: {e}")
            raise

        # ROS2 publisher
        self.pub = self.create_publisher(CTRL, "ctrl_command", 10)
        self.current_mode = 0
        self.lock = threading.Lock()

        # 定时 200ms 发布消息
        self.timer = self.create_timer(0.2, self.publish_ctrl_command)

        # 独立线程监听按键
        self.key_thread = threading.Thread(target=self.key_listener, daemon=True)
        self.key_thread.start()

    def _set_non_blocking(self, fd):
        """设置文件描述符为非阻塞模式"""
        flags = fcntl.fcntl(fd, fcntl.F_GETFL)
        # 现在可以正常使用os模块了
        fcntl.fcntl(fd, fcntl.F_SETFL, flags | os.O_NONBLOCK)

    def find_target_device(self):
        """查找指定 Vendor/Product 的设备，优先选择键盘类型"""
        self.get_logger().info(
            f"查找遥控器设备 ({TARGET_VENDOR:04x}:{TARGET_PRODUCT:04x})..."
        )
        keyboard_candidates = []  # 优先保存键盘类型设备

        for path in list_devices():
            try:
                dev = InputDevice(path)
                info = dev.info
                # 检查厂商和产品ID是否匹配
                if info.vendor == TARGET_VENDOR and info.product == TARGET_PRODUCT:
                    self.get_logger().info(f"找到匹配设备: {dev.name} ({path})")
                    # 检查是否是键盘类型设备
                    capabilities = dev.capabilities()
                    if ecodes.EV_KEY in capabilities:
                        # 优先选择明确标识为Keyboard的设备
                        if "keyboard" in dev.name.lower():
                            return path  # 直接返回键盘设备
                        keyboard_candidates.append(path)
                        self.get_logger().info(
                            f"找到匹配的键盘设备: {dev.name} ({path})"
                        )
            except Exception as e:
                self.get_logger().warning(f"访问设备 {path} 时出错: {str(e)}")

        # 优先返回找到的第一个键盘类型的设备
        if keyboard_candidates:
            return keyboard_candidates[0]

        self.get_logger().warning("没有找到目标遥控器设备")
        return None

    def publish_ctrl_command(self):
        msg = CTRL()
        with self.lock:
            msg.mode = self.current_mode
        msg.cockpit_flag = 0
        msg.gear = 0
        msg.throttle = 0.0
        msg.brake = 0.0
        msg.steering = 0.0
        msg.epb = 0
        msg.door = 0
        msg.turn_light = 0
        msg.warning = 0
        msg.headlamp = 0
        msg.fog_light = 0
        msg.ac_mode = 0
        msg.continue_run = 0
        self.pub.publish(msg)

    def key_listener(self):
        """处理按键事件，拦截所有不相关的按键"""
        for event in self.dev.read_loop():
            # 拦截所有事件，不向系统传递
            if event.type == ecodes.EV_KEY:
                key_event = categorize(event)
                keycode = key_event.keycode
                if isinstance(keycode, list):
                    keycode = keycode[0]

                # 只处理我们关心的按键，其他全部屏蔽
                if key_event.keystate == key_event.key_down:
                    if keycode == "KEY_UP":
                        self.switch_mode(1, "自动驾驶模式")
                    elif keycode == "KEY_DOWN":
                        self.switch_mode(0, "手动模式")
                    else:
                        # 明确拦截其他按键，不做任何处理就是最好的屏蔽
                        self.get_logger().debug(f"已拦截按键: {keycode}")
            else:
                # 忽略非按键事件
                self.get_logger().debug(f"已拦截非按键事件: {event.type}")

    def switch_mode(self, mode, desc):
        with self.lock:
            if self.current_mode != mode:
                self.current_mode = mode
                self.get_logger().info(f"切换到 {desc} (模式值: {mode})")


def main(args=None):
    rclpy.init(args=args)
    node = None
    try:
        node = RemoteControlNode()
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    except Exception as e:
        print(f"节点运行异常: {e}")
    finally:
        if node is not None:
            try:
                # 释放设备独占
                node.dev.ungrab()
                fcntl.flock(node.dev.fd, fcntl.LOCK_UN)
            except Exception:
                pass
            node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
