#!/usr/bin/env python3
"""Receive safe short-lived drive commands from the PC and publish ROS Twist."""

from __future__ import annotations

import argparse
import json
import math
import socket
import time

import rclpy
from geometry_msgs.msg import Twist


def main() -> None:
    parser = argparse.ArgumentParser(description="PC UDP to ROS cmd_vel_task bridge")
    parser.add_argument("--bind", default="0.0.0.0")
    parser.add_argument("--port", type=int, default=8765)
    parser.add_argument("--topic", default="/cmd_vel_task")
    parser.add_argument("--timeout", type=float, default=0.35)
    args = parser.parse_args()

    rclpy.init()
    node = rclpy.create_node("x5_udp_cmd_bridge")
    publisher = node.create_publisher(Twist, args.topic, 10)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    sock.bind((args.bind, args.port))
    sock.settimeout(0.05)
    last_packet = 0.0
    current = (0.0, 0.0)
    node.get_logger().info(f"UDP {args.bind}:{args.port} -> {args.topic}, timeout={args.timeout}s")

    try:
        while rclpy.ok():
            try:
                raw, _address = sock.recvfrom(1024)
                data = json.loads(raw.decode("utf-8"))
                v = float(data.get("v", 0.0))
                w = float(data.get("w", 0.0))
                if not (math.isfinite(v) and math.isfinite(w)):
                    raise ValueError("non-finite command")
                current = (max(-0.35, min(0.35, v)), max(-1.6, min(1.6, w)))
                last_packet = time.monotonic()
            except socket.timeout:
                pass
            except (ValueError, TypeError, json.JSONDecodeError) as exc:
                node.get_logger().warn(f"ignore invalid UDP command: {exc}")

            if time.monotonic() - last_packet > args.timeout:
                current = (0.0, 0.0)
            msg = Twist()
            msg.linear.x, msg.angular.z = current
            publisher.publish(msg)
            rclpy.spin_once(node, timeout_sec=0.0)
    finally:
        publisher.publish(Twist())
        sock.close()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
