#!/usr/bin/env python3
"""Send a goal pose to car_nav_lite via /goal_pose topic.

Usage:
  python3 test_goal.py                           # default goal
  python3 test_goal.py --x 1.0 --y -1.5          # custom goal
  python3 test_goal.py --auto                    # auto-cycle multiple goals
"""

import time
import argparse
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped


GOALS = [
    # (x, y) — world coords in map frame (origin at center, 5m×5m)
    # Car spawns near (-2.0, -2.3) facing +x

    # Goal A: straight ahead ~3m, slight right
    # Should trigger multi-pass inflation (cone at -1.58,-2.31 blocks straight path)
    (0.98, -1.49),

    # Goal B: far corner
    (2.0, 2.0),

    # Goal C: left side
    (-1.5, 0.5),
]


class GoalSender(Node):
    def __init__(self):
        super().__init__("goal_sender")
        self.pub = self.create_publisher(PoseStamped, "/goal_pose", 10)
        self.idx = 0

    def send(self, x, y, yaw=0.0):
        msg = PoseStamped()
        msg.header.frame_id = "map"
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.pose.position.x = float(x)
        msg.pose.position.y = float(y)
        msg.pose.position.z = 0.0
        # yaw encoded as quaternion (scalar-last: qx,qy,qz,qw)
        msg.pose.orientation.z = float(__import__('math').sin(yaw / 2.0))
        msg.pose.orientation.w = float(__import__('math').cos(yaw / 2.0))
        self.pub.publish(msg)
        self.get_logger().info(f"Sent goal: ({x:.2f}, {y:.2f}, yaw={yaw:.2f})")


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--x", type=float, default=None)
    parser.add_argument("--y", type=float, default=None)
    parser.add_argument("--yaw", type=float, default=0.0)
    parser.add_argument("--auto", action="store_true",
                        help="Auto-cycle through test goals")
    parser.add_argument("--interval", type=float, default=30.0,
                        help="Seconds between auto goals")
    args = parser.parse_args()

    rclpy.init()
    node = GoalSender()

    if args.auto:
        node.get_logger().info(f"Auto mode: {len(GOALS)} goals, {args.interval}s interval")
        while rclpy.ok():
            x, y = GOALS[node.idx % len(GOALS)]
            node.send(x, y, 0.0)
            node.idx += 1
            time.sleep(args.interval)
    elif args.x is not None and args.y is not None:
        node.send(args.x, args.y, args.yaw)
    else:
        # Default: use first goal
        x, y = GOALS[0]
        node.get_logger().info(f"No args — sending default goal ({x:.2f}, {y:.2f})")
        node.send(x, y, 0.0)

    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()