#!/usr/bin/env python3
"""
MageDok Touch Input Monitor
Verifies USB touch device is recognized and functional
"""

import os
import subprocess
import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Bool


class TouchMonitor(Node):
    def __init__(self):
        super().__init__('touch_monitor')

        self.declare_parameter('device_name', 'MageDok Touch')
        self.declare_parameter('poll_interval', 0.1)

        self.device_name = self.get_parameter('device_name').value
        self.poll_interval = self.get_parameter('poll_interval').value

        self.touch_status_pub = self.create_publisher(Bool, '/magedok/touch_status', 10)
        self.device_info_pub = self.create_publisher(String, '/magedok/device_info', 10)

        self.get_logger().info(f'Touch Monitor: Scanning for {self.device_name}...')
        self.detect_touch_device()

        # Publish status every 2 seconds
        self.create_timer(2.0, self.publish_status)

    def detect_touch_device(self):
        """Detect MageDok touch device via USB"""
        try:
            # Check lsusb for MageDok or eGTouch device
            result = subprocess.run(['lsusb'], capture_output=True, text=True, timeout=5)
            lines = result.stdout.split('\n')

            for line in lines:
                if 'eGTouch' in line or 'EETI' in line or 'MageDok' in line or 'touch' in line.lower():
                    self.get_logger().info(f'✓ Touch device found: {line.strip()}')
                    msg = String()
                    msg.data = line.strip()
                    self.device_info_pub.publish(msg)
                    return True

            # Fallback: check input devices
            result = subprocess.run(['grep', '-l', 'eGTouch\|EETI\|MageDok', '/proc/bus/input/devices'],
                                    capture_output=True, text=True, timeout=5)
            if result.returncode == 0:
                self.get_logger().info('✓ Touch device registered in /proc/bus/input/devices')
                return True

            self.get_logger().warn('⚠ Touch device not detected — ensure USB connection is secure')
            return False

        except Exception as e:
            self.get_logger().error(f'Device detection failed: {e}')
            return False

    def publish_status(self):
        """Publish current touch device status"""
        try:
            result = subprocess.run(['ls', '/dev/magedok-touch'], capture_output=True, timeout=2)
            status = Bool()
            status.data = (result.returncode == 0)
            self.touch_status_pub.publish(status)

            if status.data:
                self.get_logger().debug('Touch device: ACTIVE')
            else:
                self.get_logger().warn('Touch device: NOT DETECTED')
        except Exception as e:
            status = Bool()
            status.data = False
            self.touch_status_pub.publish(status)


def main(args=None):
    rclpy.init(args=args)
    monitor = TouchMonitor()
    rclpy.spin(monitor)
    rclpy.shutdown()


if __name__ == '__main__':
    main()
