#!/usr/bin/env python3
"""
UDP → /cmd_vel bridge for keyboard control.
Receives (float64 lin_x, float64 ang_z) from Windows client.

Deploy on RDKx5: /home/sunrise/yiliao_ws/udp_to_cmdvel.py
Run: python3 udp_to_cmdvel.py
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import socket
import struct
import sys


class UDPToCmdVel(Node):
    def __init__(self, port=9999):
        super().__init__("udp_to_cmdvel")
        self.pub = self.create_publisher(Twist, "/cmd_vel", 10)
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.sock.bind(("0.0.0.0", port))
        self.sock.settimeout(0.1)
        self.get_logger().info("Listening on UDP port %d → /cmd_vel" % port)
        self.create_timer(0.05, self.loop)

    def loop(self):
        try:
            data, addr = self.sock.recvfrom(16)
            lin, ang = struct.unpack('dd', data)
            msg = Twist()
            msg.linear.x = lin
            msg.angular.z = ang
            self.pub.publish(msg)
        except socket.timeout:
            pass

    def destroy_node(self):
        self.sock.close()
        super().destroy_node()


def main():
    rclpy.init()
    port = int(sys.argv[1]) if len(sys.argv) > 1 else 9999
    node = UDPToCmdVel(port=port)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()