#!/usr/bin/env python3
"""
publish_sine_path.py — 基于机器人当前位姿发布一次正弦路径到 /plan

用法:
  python3 publish_sine_path.py [amplitude] [wavelength] [length]
  默认: amplitude=0.8m  wavelength=3.0m  length=8.0m

路径在机器人前方展开:
  local_x 沿机器人朝向正前方
  local_y = A * sin(2pi * local_x / wavelength)
  再旋转变换到 odom 坐标系

依赖: /odom 话题
"""

import sys
import math
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path, Odometry
from geometry_msgs.msg import PoseStamped


class SinePathPublisher(Node):
    def __init__(self, amplitude, wavelength, length):
        super().__init__("sine_path_publisher")

        self.A = amplitude
        self.wavelength = wavelength
        self.length = length
        self.step = 0.05

        self.robot_x = 0.0
        self.robot_y = 0.0
        self.robot_yaw = 0.0
        self.has_odom = False

        self.pub = self.create_publisher(Path, "/plan", 10)
        self.odom_sub = self.create_subscription(
            Odometry, "/odom", self.odom_cb, 10)

    def odom_cb(self, msg):
        self.robot_x = msg.pose.pose.position.x
        self.robot_y = msg.pose.pose.position.y
        qx = msg.pose.pose.orientation.x
        qy = msg.pose.pose.orientation.y
        qz = msg.pose.pose.orientation.z
        qw = msg.pose.pose.orientation.w
        self.robot_yaw = math.atan2(
            2.0 * (qw * qz + qx * qy),
            1.0 - 2.0 * (qy * qy + qz * qz))
        self.has_odom = True

    def build_and_publish(self):
        cos_yaw = math.cos(self.robot_yaw)
        sin_yaw = math.sin(self.robot_yaw)

        path = Path()
        path.header.frame_id = "odom"
        path.header.stamp = self.get_clock().now().to_msg()

        lx = 0.0
        while lx <= self.length:
            ly = self.A * math.sin(2.0 * math.pi * lx / self.wavelength)

            dy_dx = self.A * (2.0 * math.pi / self.wavelength) * \
                math.cos(2.0 * math.pi * lx / self.wavelength)
            local_yaw = math.atan2(dy_dx, 1.0)

            wx = self.robot_x + lx * cos_yaw - ly * sin_yaw
            wy = self.robot_y + lx * sin_yaw + ly * cos_yaw
            world_yaw = self.robot_yaw + local_yaw

            pose = PoseStamped()
            pose.header.frame_id = "odom"
            pose.pose.position.x = wx
            pose.pose.position.y = wy
            pose.pose.position.z = 0.0
            pose.pose.orientation.z = math.sin(world_yaw / 2.0)
            pose.pose.orientation.w = math.cos(world_yaw / 2.0)
            path.poses.append(pose)

            lx += self.step

        self.pub.publish(path)
        self.get_logger().info(
            f"Published sine path: {len(path.poses)} waypoints "
            f"from ({self.robot_x:.2f}, {self.robot_y:.2f}, {math.degrees(self.robot_yaw):.0f}°)"
        )


def main():
    rclpy.init()

    A = float(sys.argv[1]) if len(sys.argv) > 1 else 0.8
    wavelength = float(sys.argv[2]) if len(sys.argv) > 2 else 3.0
    length = float(sys.argv[3]) if len(sys.argv) > 3 else 8.0

    node = SinePathPublisher(A, wavelength, length)

    # 等待 odom (最多 3 秒)
    timeout = node.get_clock().now() + rclpy.duration.Duration(seconds=3)
    while not node.has_odom and node.get_clock().now() < timeout:
        rclpy.spin_once(node, timeout_sec=0.05)

    if not node.has_odom:
        node.get_logger().error("No /odom received within 3s — aborting")
        node.destroy_node()
        rclpy.shutdown()
        sys.exit(1)

    # 额外 spin 一下让其他订阅就绪
    rclpy.spin_once(node, timeout_sec=0.5)

    node.build_and_publish()

    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()