#!/usr/bin/env python3
"""static_map_publisher.py — 从 YAML+PNG 加载并发布 /map OccupancyGrid。"""

import os, math
import numpy as np
from PIL import Image
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, DurabilityPolicy
from nav_msgs.msg import OccupancyGrid, MapMetaData
from geometry_msgs.msg import Pose
import yaml


class StaticMapPublisher(Node):
    def __init__(self):
        super().__init__('static_map_publisher')
        self.declare_parameter('yaml_filename', '')
        self.declare_parameter('publish_rate', 1.0)
        self.declare_parameter('map_frame', 'map')

        path = self.get_parameter('yaml_filename').value
        rate = self.get_parameter('publish_rate').value
        frame = self.get_parameter('map_frame').value
        if not path:
            raise RuntimeError('yaml_filename required')

        self._grid_ = self._load(path, frame)
        map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self._pub_ = self.create_publisher(OccupancyGrid, '/map', map_qos)
        self._timer_ = self.create_timer(1.0 / max(0.1, rate), self._publish)
        self.get_logger().info(f'map published on /map ({rate} Hz)')

    def _load(self, yaml_path, frame):
        d = os.path.dirname(os.path.abspath(yaml_path))
        with open(yaml_path) as f:
            cfg = yaml.safe_load(f)
        img = Image.open(os.path.join(d, cfg['image'])).convert('L')
        w, h = img.size
        data = np.array(img, dtype=np.int32)
        mode = cfg.get('mode', 'trinary')
        negate = int(cfg.get('negate', 0))
        occ_th = float(cfg.get('occupied_thresh', 0.65))
        free_th = float(cfg.get('free_thresh', 0.196))

        if mode == 'trinary':
            grid = np.full_like(data, -1, dtype=np.int8)
            grid[data == 0] = 100
            grid[data >= 254] = 0
        else:
            if negate:
                data = 255 - data
            grid = np.full_like(data, -1, dtype=np.int8)
            grid[data > occ_th * 255] = 100
            grid[data < free_th * 255] = 0

        grid = np.flipud(grid)
        origin = cfg.get('origin', [0, 0, 0])
        msg = OccupancyGrid()
        msg.header.frame_id = frame
        msg.info = MapMetaData()
        msg.info.resolution = float(cfg.get('resolution', 0.05))
        msg.info.width = w
        msg.info.height = h
        msg.info.origin = Pose()
        msg.info.origin.position.x = float(origin[0])
        msg.info.origin.position.y = float(origin[1])
        yaw = float(origin[2]) if len(origin) > 2 else 0.0
        msg.info.origin.orientation.w = math.cos(yaw / 2)
        msg.info.origin.orientation.z = math.sin(yaw / 2)
        msg.data = grid.flatten().tolist()
        return msg

    def _publish(self):
        self._grid_.header.stamp = self.get_clock().now().to_msg()
        self._pub_.publish(self._grid_)


def main():
    rclpy.init()
    try:
        rclpy.spin(StaticMapPublisher())
    except RuntimeError as e:
        rclpy.get_logger('static_map_publisher').fatal(str(e))
    except KeyboardInterrupt:
        pass
    finally:
        rclpy.shutdown()


if __name__ == '__main__':
    main()
