#!/usr/bin/env python3
"""local_map_pub — publish a converted map on /map so RViz can preview it LOCALLY.

Used by the Pudu Map GUI's "RViz: preview converted map" button: no robot, no
rosbridge, no map_server needed — just the workstation's ROS (Jazzy) + the
converted map.yaml. Republishes every 2 s (works with RViz's default volatile
Map subscription regardless of durability settings).

    python3 local_map_pub.py /path/to/map.yaml
"""
import os
import sys

import numpy as np
import rclpy
import yaml
from nav_msgs.msg import OccupancyGrid
from PIL import Image
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile


def load_grid(yaml_path):
    with open(yaml_path) as f:
        meta = yaml.safe_load(f)
    img_path = meta["image"]
    if not os.path.isabs(img_path):
        img_path = os.path.join(os.path.dirname(os.path.abspath(yaml_path)), img_path)
    gray = np.array(Image.open(img_path).convert("L")).astype(np.float32)
    free_t = float(meta.get("free_thresh", 0.196))
    occ_t = float(meta.get("occupied_thresh", 0.65))
    if int(meta.get("negate", 0)):
        p = gray / 255.0
    else:
        p = (255.0 - gray) / 255.0
    occ = np.full(gray.shape, -1, dtype=np.int8)
    occ[p <= free_t] = 0
    occ[p >= occ_t] = 100
    # pgm row 0 = top; OccupancyGrid row 0 = bottom -> flip vertically
    occ = occ[::-1, :]
    return meta, occ


class LocalMapPub(Node):
    def __init__(self, yaml_path):
        super().__init__("pudu_gui_local_map_pub")
        meta, occ = load_grid(yaml_path)
        self._msg = OccupancyGrid()
        self._msg.header.frame_id = "map"
        self._msg.info.resolution = float(meta["resolution"])
        self._msg.info.height, self._msg.info.width = occ.shape
        ox, oy = float(meta["origin"][0]), float(meta["origin"][1])
        self._msg.info.origin.position.x = ox
        self._msg.info.origin.position.y = oy
        self._msg.info.origin.orientation.w = 1.0
        self._msg.data = occ.reshape(-1).tolist()
        qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL)
        self._pub = self.create_publisher(OccupancyGrid, "/map", qos)
        self.create_timer(2.0, self._tick)
        self._tick()
        self.get_logger().info(
            f"publishing {self._msg.info.width}x{self._msg.info.height} @ "
            f"{self._msg.info.resolution} m/px, origin ({ox}, {oy}) from {yaml_path}")

    def _tick(self):
        self._msg.header.stamp = self.get_clock().now().to_msg()
        self._pub.publish(self._msg)


def main():
    if len(sys.argv) < 2:
        sys.exit("usage: local_map_pub.py /path/to/map.yaml")
    rclpy.init()
    node = LocalMapPub(sys.argv[1])
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass


if __name__ == "__main__":
    main()