#!/usr/bin/env python3
"""在 rviz 显示所有示教点位：读 taught_points.yaml，发布 MarkerArray。

每个点画：① 箭头 = 位置 + 朝向(yaw)；② 文字 = 点名。frame_id = map。
周期性重读文件并发布，因此示教(teach_points)时新增/删除的点会实时反映到 rviz。

rviz 里加 MarkerArray 显示、话题 /taught_points（localization.rviz 已内置该显示）。

用法：
  ros2 run craic_localization show_points --ros-args \
      -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
"""

import math
import os

import rclpy
import yaml
from geometry_msgs.msg import Quaternion
from rclpy.node import Node
from visualization_msgs.msg import Marker, MarkerArray


def yaw_to_quat(yaw):
    q = Quaternion()
    q.z = math.sin(yaw / 2.0)
    q.w = math.cos(yaw / 2.0)
    return q


class ShowPoints(Node):
    def __init__(self):
        super().__init__('show_points')
        self.points_file = os.path.abspath(
            self.declare_parameter('points_file', 'taught_points.yaml').value)
        self.frame_id = self.declare_parameter('frame_id', 'map').value
        self.rate = float(self.declare_parameter('rate', 1.0).value)

        self.pub = self.create_publisher(MarkerArray, 'taught_points', 1)
        self.timer = self.create_timer(1.0 / self.rate, self._tick)
        self.get_logger().info(
            f'show_points: {self.points_file} -> /taught_points (frame={self.frame_id})')

    def _load(self):
        try:
            with open(self.points_file, 'r', encoding='utf-8') as f:
                data = yaml.safe_load(f) or {}
            return dict(data.get('points', {}))
        except FileNotFoundError:
            return {}
        except Exception as exc:  # noqa: BLE001
            self.get_logger().warn(f'读取点位失败: {exc}', throttle_duration_sec=5.0)
            return {}

    def _tick(self):
        pts = self._load()
        now = self.get_clock().now().to_msg()
        arr = MarkerArray()

        # 先清除上一帧（与新增放同一条消息，rviz 一次性处理，无闪烁），自动反映删除
        clear = Marker()
        clear.header.frame_id = self.frame_id
        clear.action = Marker.DELETEALL
        arr.markers.append(clear)

        for i, (name, p) in enumerate(pts.items()):
            try:
                x, y = float(p['x']), float(p['y'])
                yaw = float(p.get('yaw', 0.0))
            except (KeyError, TypeError, ValueError):
                continue

            # 箭头：位置 + 朝向
            a = Marker()
            a.header.frame_id = self.frame_id
            a.header.stamp = now
            a.ns = 'arrow'
            a.id = i
            a.type = Marker.ARROW
            a.action = Marker.ADD
            a.pose.position.x = x
            a.pose.position.y = y
            a.pose.position.z = 0.05
            a.pose.orientation = yaw_to_quat(yaw)
            a.scale.x = 0.25   # 杆长
            a.scale.y = 0.04   # 杆径
            a.scale.z = 0.04   # 头径
            a.color.r, a.color.g, a.color.b, a.color.a = 0.1, 0.8, 1.0, 1.0  # 青色
            arr.markers.append(a)

            # 文字：点名
            t = Marker()
            t.header.frame_id = self.frame_id
            t.header.stamp = now
            t.ns = 'label'
            t.id = i
            t.type = Marker.TEXT_VIEW_FACING
            t.action = Marker.ADD
            t.pose.position.x = x
            t.pose.position.y = y
            t.pose.position.z = 0.22
            t.pose.orientation.w = 1.0
            t.scale.z = 0.12   # 字高
            t.color.r, t.color.g, t.color.b, t.color.a = 1.0, 1.0, 1.0, 1.0
            t.text = name
            arr.markers.append(t)

        self.pub.publish(arr)


def main(args=None):
    rclpy.init(args=args)
    node = ShowPoints()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()


if __name__ == '__main__':
    main()
