#!/usr/bin/env python3
"""One-shot /map -> pgm+yaml exporter. Run INSIDE the robot's VSLAM/nav
container (read-only w.r.t. the robot; subscribes once, writes /data, exits).

    source /opt/ros/foxy/setup.bash && timeout 30 python3 map_export_once.py
"""
import math
import sys

import rclpy
from nav_msgs.msg import OccupancyGrid
from rclpy.node import Node
from rclpy.qos import QoSDurabilityPolicy, QoSProfile, QoSReliabilityPolicy

OUT = "/data/map_export"


class Grab(Node):
    def __init__(self):
        super().__init__("map_export_once")
        qos = QoSProfile(depth=1,
                         reliability=QoSReliabilityPolicy.RELIABLE,
                         durability=QoSDurabilityPolicy.TRANSIENT_LOCAL)
        self.create_subscription(OccupancyGrid, "/map", self.cb, qos)
        self.done = False

    def cb(self, msg):
        if self.done:
            return
        self.done = True
        w, h, res = msg.info.width, msg.info.height, msg.info.resolution
        ox, oy = msg.info.origin.position.x, msg.info.origin.position.y
        q = msg.info.origin.orientation
        yaw = math.atan2(2 * (q.w * q.z + q.x * q.y),
                         1 - 2 * (q.y * q.y + q.z * q.z))
        d = msg.data
        # map_saver convention: unknown(-1)->205, free->254, occupied->0
        px = bytearray(w * h)
        for i in range(w * h):
            v = d[i]
            px[i] = 205 if v < 0 else (0 if v >= 65 else 254)
        with open(OUT + ".pgm", "wb") as f:
            f.write(b"P5\n%d %d\n255\n" % (w, h))
            for y in range(h - 1, -1, -1):      # grid row 0 = bottom -> flip
                f.write(bytes(px[y * w:(y + 1) * w]))
        with open(OUT + ".yaml", "w") as f:
            f.write("image: map_export.pgm\nresolution: %.6f\n"
                    "origin: [%.6f, %.6f, %.6f]\nnegate: 0\n"
                    "occupied_thresh: 0.65\nfree_thresh: 0.196\n"
                    % (res, ox, oy, yaw))
        print("EXPORTED %dx%d @ %.3fm origin=(%.2f, %.2f, %.2f)"
              % (w, h, res, ox, oy, yaw), flush=True)
        rclpy.shutdown()


def main():
    rclpy.init()
    rclpy.spin(Grab())


if __name__ == "__main__":
    sys.exit(main())