#!/usr/bin/env python3
"""Inspect a ROS map YAML/PGM pair and validate its metric dimensions."""

import argparse
from pathlib import Path
import sys

import yaml


def _pgm_token(stream):
    while True:
        byte = stream.read(1)
        if not byte:
            raise ValueError("unexpected end of PGM header")
        if byte.isspace():
            continue
        if byte == b"#":
            stream.readline()
            continue
        token = byte
        while True:
            byte = stream.read(1)
            if not byte or byte.isspace():
                return token.decode("ascii")
            token += byte


def read_pgm_header(path):
    with path.open("rb") as stream:
        magic = _pgm_token(stream)
        if magic not in ("P2", "P5"):
            raise ValueError(f"unsupported image format {magic}, expected P2/P5")
        width = int(_pgm_token(stream))
        height = int(_pgm_token(stream))
        max_value = int(_pgm_token(stream))
    return magic, width, height, max_value


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("map_yaml", type=Path)
    parser.add_argument("--expected-width", type=float, default=5.0)
    parser.add_argument("--expected-height", type=float, default=5.0)
    parser.add_argument("--tolerance", type=float, default=0.15)
    parser.add_argument("--no-check", action="store_true")
    args = parser.parse_args()

    data = yaml.safe_load(args.map_yaml.read_text(encoding="utf-8")) or {}
    image = Path(data["image"])
    if not image.is_absolute():
        image = args.map_yaml.parent / image
    magic, pixels_x, pixels_y, max_value = read_pgm_header(image)
    resolution = float(data["resolution"])
    metric_x = pixels_x * resolution
    metric_y = pixels_y * resolution
    origin = data.get("origin", [0.0, 0.0, 0.0])

    print(f"yaml:       {args.map_yaml}")
    print(f"image:      {image}")
    print(f"format:     {magic}, max_value={max_value}")
    print(f"pixels:     {pixels_x} x {pixels_y}")
    print(f"resolution: {resolution:.6f} m/pixel")
    print(f"size:       {metric_x:.3f} x {metric_y:.3f} m")
    print(f"origin:     {origin}")

    if args.no_check:
        return 0
    if (
        abs(metric_x - args.expected_width) > args.tolerance
        or abs(metric_y - args.expected_height) > args.tolerance
    ):
        print(
            "FAIL: expected %.3f x %.3f m +/- %.3f m"
            % (args.expected_width, args.expected_height, args.tolerance)
        )
        return 1
    print("PASS: map dimensions match the expected metric size")
    return 0


if __name__ == "__main__":
    raise SystemExit(main())
