#!/usr/bin/env python3
"""Publish a known current pose and then a navigation goal."""

import argparse
import math
import time

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


def fill_yaw_quaternion(q, yaw):
    q.x = 0.0
    q.y = 0.0
    q.z = math.sin(0.5 * yaw)
    q.w = math.cos(0.5 * yaw)


class StartGoalPublisher(Node):
    def __init__(self, args):
        super().__init__("send_start_goal")
        qos = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )
        self.start_pub = self.create_publisher(
            PoseWithCovarianceStamped, args.initialpose_topic, qos
        )
        self.goal_pub = self.create_publisher(PoseStamped, args.goal_topic, qos)
        self.frame = args.frame

        self.start = PoseWithCovarianceStamped()
        self.start.header.frame_id = args.frame
        self.start.pose.pose.position.x = args.start_x
        self.start.pose.pose.position.y = args.start_y
        fill_yaw_quaternion(self.start.pose.pose.orientation, args.start_yaw)
        self.start.pose.covariance[0] = args.xy_covariance
        self.start.pose.covariance[7] = args.xy_covariance
        self.start.pose.covariance[35] = args.yaw_covariance

        self.goal = PoseStamped()
        self.goal.header.frame_id = args.frame
        self.goal.pose.position.x = args.goal_x
        self.goal.pose.position.y = args.goal_y
        fill_yaw_quaternion(self.goal.pose.orientation, args.goal_yaw)

    def publish_start(self):
        self.start.header.stamp = self.get_clock().now().to_msg()
        self.start_pub.publish(self.start)

    def publish_goal(self):
        self.goal.header.stamp = self.get_clock().now().to_msg()
        self.goal_pub.publish(self.goal)


def wait_for_subscribers(node, timeout):
    deadline = time.monotonic() + max(0.5, timeout)
    while time.monotonic() < deadline:
        rclpy.spin_once(node, timeout_sec=0.05)
        if (
            node.start_pub.get_subscription_count() > 0
            and node.goal_pub.get_subscription_count() > 0
        ):
            return True
    return False


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("start_x", type=float)
    parser.add_argument("start_y", type=float)
    parser.add_argument("start_yaw", type=float)
    parser.add_argument("goal_x", type=float)
    parser.add_argument("goal_y", type=float)
    parser.add_argument("goal_yaw", type=float, nargs="?", default=0.0)
    parser.add_argument("--frame", default="map")
    parser.add_argument("--initialpose-topic", default="/initialpose")
    parser.add_argument("--goal-topic", default="/goal_pose")
    parser.add_argument("--timeout", type=float, default=5.0)
    parser.add_argument("--repeat", type=int, default=5)
    parser.add_argument("--interval", type=float, default=0.10)
    parser.add_argument("--goal-delay", type=float, default=0.30)
    parser.add_argument("--xy-covariance", type=float, default=0.0025)
    parser.add_argument("--yaw-covariance", type=float, default=0.0076)
    args = parser.parse_args()

    rclpy.init()
    node = StartGoalPublisher(args)
    try:
        if not wait_for_subscribers(node, args.timeout):
            node.get_logger().error(
                "missing subscribers: /initialpose=%d /goal_pose=%d"
                % (
                    node.start_pub.get_subscription_count(),
                    node.goal_pub.get_subscription_count(),
                )
            )
            return 1

        for _ in range(max(1, args.repeat)):
            node.publish_start()
            rclpy.spin_once(node, timeout_sec=0.02)
            time.sleep(max(0.02, args.interval))

        time.sleep(max(0.0, args.goal_delay))
        for _ in range(max(1, args.repeat)):
            node.publish_goal()
            rclpy.spin_once(node, timeout_sec=0.02)
            time.sleep(max(0.02, args.interval))

        node.get_logger().info(
            "published start %s=(%.3f, %.3f, %.3f) and goal %s=(%.3f, %.3f, %.3f)"
            % (
                args.frame,
                args.start_x,
                args.start_y,
                args.start_yaw,
                args.frame,
                args.goal_x,
                args.goal_y,
                args.goal_yaw,
            )
        )
        return 0
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


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