#!/usr/bin/env python3
"""Publish one map-frame goal for the real-robot Hybrid A* stack."""

import argparse
import math
import time

import rclpy
from geometry_msgs.msg import PoseStamped
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy


class GoalPublisher(Node):
    def __init__(self, x, y, yaw, topic, frame_id):
        super().__init__("send_goal")
        goal_qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.publisher = self.create_publisher(PoseStamped, topic, goal_qos)
        self.goal = PoseStamped()
        self.goal.header.frame_id = frame_id
        self.goal.pose.position.x = x
        self.goal.pose.position.y = y
        self.goal.pose.orientation.z = math.sin(yaw * 0.5)
        self.goal.pose.orientation.w = math.cos(yaw * 0.5)

    def publish_once(self):
        self.goal.header.stamp = self.get_clock().now().to_msg()
        self.publisher.publish(self.goal)


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("x", type=float)
    parser.add_argument("y", type=float)
    parser.add_argument("yaw", type=float, nargs="?", default=0.0)
    parser.add_argument("--topic", default="/goal_pose")
    parser.add_argument("--frame", default="map")
    parser.add_argument("--timeout", type=float, default=8.0)
    parser.add_argument("--repeat", type=int, default=3)
    args = parser.parse_args()

    rclpy.init()
    node = GoalPublisher(args.x, args.y, args.yaw, args.topic, args.frame)
    deadline = time.monotonic() + max(0.5, args.timeout)
    published = 0
    while time.monotonic() < deadline:
        rclpy.spin_once(node, timeout_sec=0.05)
        if node.publisher.get_subscription_count() == 0:
            continue
        node.publish_once()
        published += 1
        time.sleep(0.10)
        if published >= max(1, args.repeat):
            break
    if published == 0:
        node.get_logger().error("no /goal_pose subscriber discovered before timeout")
        node.destroy_node()
        rclpy.shutdown()
        return 1
    node.get_logger().info(
        f"published goal {published} times "
        f"{args.frame}=({args.x:.3f}, {args.y:.3f}, {args.yaw:.3f} rad)"
    )
    node.destroy_node()
    if rclpy.ok():
        rclpy.shutdown()
    return 0


if __name__ == "__main__":
    raise SystemExit(main())
