"""
gamepad_teleop_node.py — PS5 DualSense Bluetooth gamepad teleoperation for SaltyBot.

Reads from /dev/input/js0 (gamepad input device) and maps to velocity commands.
Publishes /cmd_vel (Twist) and /saltybot/teleop_active (Bool) for autonomous override.

Input Mapping (PS5 DualSense):
  Left Stick:   Linear velocity (forward/backward)
  Right Stick:  Angular velocity (turn left/right)
  L2 Trigger:   Speed multiplier decrease (30%)
  R2 Trigger:   Speed multiplier increase (100%)
  Triangle:     Toggle follow-me mode
  Square:       Emergency stop (e-stop)
  Circle:       Execute random trick
  X:            Toggle pan-tilt mode
  D-Pad Up/Down:   Manual tilt control
  D-Pad Left/Right: Manual pan control

Rumble Feedback:
  - Light rumble: Obstacle approaching (< 0.5 m)
  - Heavy rumble: Low battery (< 18 V)
"""

import struct
import threading
import time
from typing import Optional

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool, Float32


class GamepadTeleopNode(Node):
    """ROS2 node for PS5 DualSense gamepad teleoperation."""

    # Button indices (JS_EVENT_BUTTON)
    BTN_SQUARE = 0
    BTN_X = 1
    BTN_CIRCLE = 2
    BTN_TRIANGLE = 3
    BTN_L1 = 4
    BTN_R1 = 5
    BTN_L2_DIGITAL = 6
    BTN_R2_DIGITAL = 7
    BTN_SHARE = 8
    BTN_OPTIONS = 9
    BTN_L3 = 10
    BTN_R3 = 11
    BTN_PS = 12
    BTN_TOUCHPAD = 13

    # Axis indices (JS_EVENT_AXIS)
    AXIS_LX = 0  # Left stick X
    AXIS_LY = 1  # Left stick Y
    AXIS_RX = 2  # Right stick X
    AXIS_RY = 3  # Right stick Y
    AXIS_L2_ANALOG = 4  # L2 trigger analog
    AXIS_R2_ANALOG = 5  # R2 trigger analog
    AXIS_DPAD_X = 6  # D-pad horizontal
    AXIS_DPAD_Y = 7  # D-pad vertical

    def __init__(self):
        """Initialize gamepad teleop node."""
        super().__init__("gamepad_teleop_node")

        # Declare parameters
        self.declare_parameter("device", "/dev/input/js0")
        self.declare_parameter("deadzone", 0.1)
        self.declare_parameter("max_linear_vel", 2.0)
        self.declare_parameter("max_angular_vel", 2.0)
        self.declare_parameter("min_speed_mult", 0.3)
        self.declare_parameter("max_speed_mult", 1.0)
        self.declare_parameter("pan_step", 5.0)
        self.declare_parameter("tilt_step", 5.0)
        self.declare_parameter("obstacle_distance", 0.5)
        self.declare_parameter("low_battery_voltage", 18.0)

        # Get parameters
        self.device = self.get_parameter("device").value
        self.deadzone = self.get_parameter("deadzone").value
        self.max_linear_vel = self.get_parameter("max_linear_vel").value
        self.max_angular_vel = self.get_parameter("max_angular_vel").value
        self.min_speed_mult = self.get_parameter("min_speed_mult").value
        self.max_speed_mult = self.get_parameter("max_speed_mult").value
        self.pan_step = self.get_parameter("pan_step").value
        self.tilt_step = self.get_parameter("tilt_step").value
        self.obstacle_distance_threshold = self.get_parameter("obstacle_distance").value
        self.low_battery_threshold = self.get_parameter("low_battery_voltage").value

        # Publishers
        self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 1)
        self.teleop_active_pub = self.create_publisher(Bool, "/saltybot/teleop_active", 1)
        self.pan_tilt_pan_pub = self.create_publisher(Float32, "/saltybot/pan_tilt_command/pan", 1)
        self.pan_tilt_tilt_pub = self.create_publisher(Float32, "/saltybot/pan_tilt_command/tilt", 1)

        # Subscribers for feedback
        self.create_subscription(Float32, "/saltybot/obstacle_distance", self._obstacle_callback, 1)
        self.create_subscription(Float32, "/saltybot/battery_voltage", self._battery_callback, 1)

        # State variables
        self.axes = [0.0] * 8  # 8 analog axes
        self.buttons = [False] * 14  # 14 buttons
        self.speed_mult = 1.0
        self.follow_me_active = False
        self.pan_tilt_active = False
        self.teleop_enabled = True
        self.last_cmd_vel_time = time.time()

        # Feedback state
        self.last_obstacle_distance = float('inf')
        self.last_battery_voltage = 24.0
        self.rumble_active = False

        # Thread management
        self.device_fd = None
        self.reading = False
        self.reader_thread = None

        self.get_logger().info(f"Gamepad Teleop Node initialized. Listening on {self.device}")
        self.start_reading()

    def start_reading(self):
        """Start reading gamepad input in background thread."""
        self.reading = True
        self.reader_thread = threading.Thread(target=self._read_gamepad, daemon=True)
        self.reader_thread.start()

    def stop_reading(self):
        """Stop reading gamepad input."""
        self.reading = False
        if self.device_fd:
            try:
                self.device_fd.close()
            except Exception:
                pass

    def _read_gamepad(self):
        """Read gamepad events from /dev/input/jsX."""
        try:
            self.device_fd = open(self.device, "rb")
            self.get_logger().info(f"Opened gamepad device: {self.device}")
        except OSError as e:
            self.get_logger().error(f"Failed to open gamepad device {self.device}: {e}")
            return

        while self.reading:
            try:
                # JS_EVENT structure: time (4B), value (2B), type (1B), number (1B)
                event_data = self.device_fd.read(8)
                if len(event_data) < 8:
                    continue

                event_time, value, event_type, number = struct.unpack("IhBB", event_data)

                # Process button (type 0x01) or axis (type 0x02) events
                if event_type & 0x01:  # Button event
                    self._handle_button(number, value)
                elif event_type & 0x02:  # Axis event
                    self._handle_axis(number, value)

            except Exception as e:
                self.get_logger().warn(f"Error reading gamepad: {e}")
                break

    def _handle_axis(self, number: int, raw_value: int):
        """Process analog axis event."""
        # Normalize to -1.0 to 1.0
        normalized = raw_value / 32767.0
        self.axes[number] = normalized

        # Apply deadzone
        if abs(normalized) < self.deadzone:
            self.axes[number] = 0.0

        self._publish_cmd_vel()

    def _handle_button(self, number: int, pressed: bool):
        """Process button press/release."""
        self.buttons[number] = pressed

        if not pressed:
            return  # Only process button press (value=1), not release

        # Triangle: Toggle follow-me
        if number == self.BTN_TRIANGLE:
            self.follow_me_active = not self.follow_me_active
            self.get_logger().info(f"Follow-me: {self.follow_me_active}")

        # Square: E-stop
        elif number == self.BTN_SQUARE:
            self.teleop_enabled = False
            self._publish_cmd_vel()
            self.get_logger().warn("E-STOP activated")

        # Circle: Random trick
        elif number == self.BTN_CIRCLE:
            self.get_logger().info("Random trick command sent")
            # Future: publish to /saltybot/trick_command

        # X: Toggle pan-tilt
        elif number == self.BTN_X:
            self.pan_tilt_active = not self.pan_tilt_active
            self.get_logger().info(f"Pan-tilt mode: {self.pan_tilt_active}")

    def _publish_cmd_vel(self):
        """Publish velocity command from gamepad input."""
        if not self.teleop_enabled:
            # Publish zero velocity
            twist = Twist()
            self.cmd_vel_pub.publish(twist)
            self.teleop_active_pub.publish(Bool(data=False))
            return

        # Get stick inputs
        lx = self.axes[self.AXIS_LX]
        ly = -self.axes[self.AXIS_LY]  # Invert Y for forward = positive
        rx = self.axes[self.AXIS_RX]

        # Speed multiplier from triggers
        l2 = max(0.0, self.axes[self.AXIS_L2_ANALOG])
        r2 = max(0.0, self.axes[self.AXIS_R2_ANALOG])
        self.speed_mult = self.min_speed_mult + (r2 - l2) * (self.max_speed_mult - self.min_speed_mult)
        self.speed_mult = max(self.min_speed_mult, min(self.max_speed_mult, self.speed_mult))

        # Calculate velocities
        linear_vel = ly * self.max_linear_vel * self.speed_mult
        angular_vel = rx * self.max_angular_vel * self.speed_mult

        # Publish cmd_vel
        twist = Twist()
        twist.linear.x = linear_vel
        twist.angular.z = angular_vel
        self.cmd_vel_pub.publish(twist)

        # Publish teleop_active flag
        self.teleop_active_pub.publish(Bool(data=True))

        # Handle pan-tilt from D-pad
        if self.pan_tilt_active:
            dpad_x = self.axes[self.AXIS_DPAD_X]
            dpad_y = self.axes[self.AXIS_DPAD_Y]

            if dpad_x != 0:  # Left/Right
                pan_cmd = Float32(data=float(dpad_x) * self.pan_step)
                self.pan_tilt_pan_pub.publish(pan_cmd)

            if dpad_y != 0:  # Up/Down
                tilt_cmd = Float32(data=float(dpad_y) * self.tilt_step)
                self.pan_tilt_tilt_pub.publish(tilt_cmd)

        self.last_cmd_vel_time = time.time()

    def _obstacle_callback(self, msg: Float32):
        """Receive obstacle distance and trigger rumble if needed."""
        self.last_obstacle_distance = msg.data

    def _battery_callback(self, msg: Float32):
        """Receive battery voltage and trigger rumble if low."""
        self.last_battery_voltage = msg.data

    def destroy_node(self):
        """Clean up on shutdown."""
        self.stop_reading()
        super().destroy_node()


def main(args=None):
    """Main entry point."""
    rclpy.init(args=args)
    node = GamepadTeleopNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
