"""
test_nav2_amcl.py — Unit tests for Nav2 AMCL integration (Issue #655).

Tests run without a ROS2 installation:
  - amcl_nav2_params.yaml: parse, required sections, value ranges
  - saltybot_map.yaml:     parse, required keys, valid geometry
  - saltybot_map.pgm:      valid PGM header, correct dimensions
  - nav2_amcl_bringup.launch.py: import (syntax) test
"""

import os
import struct

import pytest
import yaml

# ── Paths ─────────────────────────────────────────────────────────────────────
_PKG_ROOT = os.path.join(os.path.dirname(__file__), "..")
_CFG  = os.path.join(_PKG_ROOT, "config", "amcl_nav2_params.yaml")
_MAP_YAML = os.path.join(_PKG_ROOT, "maps", "saltybot_map.yaml")
_MAP_PGM  = os.path.join(_PKG_ROOT, "maps", "saltybot_map.pgm")
_LAUNCH   = os.path.join(_PKG_ROOT, "launch", "nav2_amcl_bringup.launch.py")


# ── amcl_nav2_params.yaml ─────────────────────────────────────────────────────

def _load_params():
    with open(_CFG) as f:
        return yaml.safe_load(f)


def test_params_file_exists():
    assert os.path.isfile(_CFG)


def test_params_top_level_sections():
    params = _load_params()
    required = [
        "amcl",
        "map_server",
        "bt_navigator",
        "controller_server",
        "planner_server",
        "behavior_server",
        "global_costmap",
        "local_costmap",
        "velocity_smoother",
        "lifecycle_manager_localization",
        "lifecycle_manager_navigation",
    ]
    for section in required:
        assert section in params, f"Missing section: {section}"


def test_amcl_params_present():
    amcl = _load_params()["amcl"]["ros__parameters"]
    for key in ("min_particles", "max_particles", "alpha1", "alpha2",
                "alpha3", "alpha4", "laser_model_type", "global_frame_id",
                "odom_frame_id", "base_frame_id", "robot_model_type"):
        assert key in amcl, f"Missing amcl param: {key}"


def test_amcl_particle_bounds():
    amcl = _load_params()["amcl"]["ros__parameters"]
    assert amcl["min_particles"] >= 100
    assert amcl["max_particles"] >= amcl["min_particles"]
    assert amcl["max_particles"] <= 10000


def test_amcl_frames():
    amcl = _load_params()["amcl"]["ros__parameters"]
    assert amcl["global_frame_id"] == "map"
    assert amcl["odom_frame_id"]   == "odom"
    assert amcl["base_frame_id"]   == "base_link"


def test_amcl_motion_model():
    amcl = _load_params()["amcl"]["ros__parameters"]
    for key in ("alpha1", "alpha2", "alpha3", "alpha4"):
        assert 0.0 <= amcl[key] <= 1.0, f"{key} out of [0,1]"


def test_amcl_laser_model():
    amcl = _load_params()["amcl"]["ros__parameters"]
    assert amcl["laser_model_type"] in ("likelihood_field", "beam", "likelihood_field_prob")
    z_sum = amcl["z_hit"] + amcl["z_rand"] + amcl["z_max"] + amcl["z_short"]
    assert abs(z_sum - 1.0) < 1e-3, f"z weights sum {z_sum:.4f} ≠ 1.0"


def test_controller_server_params():
    ctrl = _load_params()["controller_server"]["ros__parameters"]
    assert ctrl["controller_frequency"] > 0
    follow = ctrl["FollowPath"]
    assert follow["max_vel_x"] > 0
    assert follow["max_vel_x"] <= 10.0    # sanity cap
    assert follow["max_vel_theta"] > 0
    assert follow["min_vel_y"] == 0.0     # differential drive: no lateral
    assert follow["vy_samples"] == 1      # differential drive


def test_planner_server_params():
    planner = _load_params()["planner_server"]["ros__parameters"]
    assert "GridBased" in planner["planner_plugins"]
    grid = planner["GridBased"]
    assert "NavfnPlanner" in grid["plugin"]
    assert grid["use_astar"] is True


def test_behavior_server_recoveries():
    behaviors = _load_params()["behavior_server"]["ros__parameters"]
    plugins = behaviors["behavior_plugins"]
    # Issue #655 requires: spin, backup, wait
    for required in ("spin", "backup", "wait"):
        assert required in plugins, f"Recovery behavior '{required}' missing"
    # Check each has a plugin entry
    for name in plugins:
        assert name in behaviors, f"behavior_plugins entry '{name}' has no config block"
        assert "plugin" in behaviors[name]


def test_global_costmap_layers():
    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
    plugins = gcm["plugins"]
    for layer in ("static_layer", "obstacle_layer", "inflation_layer"):
        assert layer in plugins, f"Global costmap missing layer: {layer}"
    # Each layer has plugin field
    for layer in plugins:
        assert layer in gcm, f"Layer {layer} has no config block"
        assert "plugin" in gcm[layer]


def test_global_costmap_scan_source():
    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
    obs = gcm["obstacle_layer"]
    assert "scan" in obs["observation_sources"]
    assert obs["scan"]["topic"] == "/scan"
    assert obs["scan"]["data_type"] == "LaserScan"


def test_local_costmap_layers():
    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
    plugins = lcm["plugins"]
    # Local costmap: obstacle + inflation (no static — rolling window)
    assert "obstacle_layer" in plugins
    assert "inflation_layer" in plugins
    assert lcm["rolling_window"] is True


def test_local_costmap_size():
    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
    assert lcm["width"]  >= 2
    assert lcm["height"] >= 2
    assert lcm["resolution"] == pytest.approx(0.05, abs=1e-6)


def test_costmap_frames():
    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
    assert gcm["global_frame"]     == "map"
    assert gcm["robot_base_frame"] == "base_link"
    assert lcm["global_frame"]     == "odom"   # local uses odom frame
    assert lcm["robot_base_frame"] == "base_link"


def test_inflation_radius_larger_than_robot():
    gcm = _load_params()["global_costmap"]["global_costmap"]["ros__parameters"]
    lcm = _load_params()["local_costmap"]["local_costmap"]["ros__parameters"]
    robot_r = gcm["robot_radius"]
    g_infl  = gcm["inflation_layer"]["inflation_radius"]
    l_infl  = lcm["inflation_layer"]["inflation_radius"]
    assert g_infl > robot_r, "Global inflation_radius must exceed robot_radius"
    assert l_infl > robot_r, "Local inflation_radius must exceed robot_radius"


def test_lifecycle_manager_localization_nodes():
    lm = _load_params()["lifecycle_manager_localization"]["ros__parameters"]
    assert "map_server" in lm["node_names"]
    assert "amcl"       in lm["node_names"]


def test_lifecycle_manager_navigation_nodes():
    lm = _load_params()["lifecycle_manager_navigation"]["ros__parameters"]
    for node in ("controller_server", "planner_server", "behavior_server", "bt_navigator"):
        assert node in lm["node_names"], f"lifecycle nav missing: {node}"


def test_velocity_smoother_limits_consistent():
    vs = _load_params()["velocity_smoother"]["ros__parameters"]
    ctrl = _load_params()["controller_server"]["ros__parameters"]
    max_vx_ctrl = ctrl["FollowPath"]["max_vel_x"]
    max_vx_vs   = vs["max_velocity"][0]
    # Smoother limit should be >= planner limit (or equal)
    assert max_vx_vs >= max_vx_ctrl - 1e-3, (
        f"velocity_smoother max_velocity[0]={max_vx_vs} < "
        f"controller max_vel_x={max_vx_ctrl}"
    )


# ── saltybot_map.yaml ─────────────────────────────────────────────────────────

def _load_map_yaml():
    with open(_MAP_YAML) as f:
        return yaml.safe_load(f)


def test_map_yaml_exists():
    assert os.path.isfile(_MAP_YAML)


def test_map_yaml_required_keys():
    m = _load_map_yaml()
    for key in ("image", "resolution", "origin", "negate",
                "occupied_thresh", "free_thresh"):
        assert key in m, f"Map YAML missing key: {key}"


def test_map_yaml_thresholds():
    m = _load_map_yaml()
    assert 0.0 < m["free_thresh"] < m["occupied_thresh"] < 1.0


def test_map_yaml_resolution():
    m = _load_map_yaml()
    assert m["resolution"] == pytest.approx(0.05, abs=1e-6)


def test_map_yaml_origin_3d():
    m = _load_map_yaml()
    assert len(m["origin"]) == 3   # [x, y, yaw]


def test_map_yaml_image_name():
    m = _load_map_yaml()
    assert m["image"].endswith(".pgm")


# ── saltybot_map.pgm ─────────────────────────────────────────────────────────

def test_pgm_exists():
    assert os.path.isfile(_MAP_PGM)


def test_pgm_valid_header():
    """P5 binary PGM: header 'P5\\nW H\\nMAXVAL\\n'."""
    with open(_MAP_PGM, "rb") as f:
        magic = f.readline().strip()
    assert magic == b"P5", f"Expected P5 PGM, got {magic}"


def test_pgm_dimensions():
    """Parse width and height from PGM header; verify > 0."""
    with open(_MAP_PGM, "rb") as f:
        f.readline()   # P5
        wh_line = f.readline()
    w_str, h_str = wh_line.decode().split()
    w, h = int(w_str), int(h_str)
    assert w > 0 and h > 0
    assert w == 200 and h == 200


def test_pgm_pixel_count():
    """File size should match header dimensions + header bytes."""
    with open(_MAP_PGM, "rb") as f:
        lines = []
        for _ in range(3):
            lines.append(f.readline())
        payload = f.read()
    w, h = map(int, lines[1].decode().split())
    assert len(payload) == w * h, (
        f"Expected {w*h} pixels, got {len(payload)}"
    )


def test_pgm_all_free_space():
    """All pixels should be 254 (near-white = free)."""
    with open(_MAP_PGM, "rb") as f:
        for _ in range(3):
            f.readline()
        data = f.read()
    assert all(b == 254 for b in data), "Not all pixels are free (254)"


# ── Launch file syntax check ──────────────────────────────────────────────────

def test_launch_file_exists():
    assert os.path.isfile(_LAUNCH)


def test_launch_file_imports_cleanly():
    """Compile launch file to check for Python syntax errors."""
    import py_compile
    py_compile.compile(_LAUNCH, doraise=True)


def test_launch_file_has_generate_function():
    """Launch file must define generate_launch_description()."""
    with open(_LAUNCH) as f:
        src = f.read()
    assert "def generate_launch_description" in src


def test_launch_file_references_amcl_params():
    """Launch file must reference amcl_nav2_params.yaml."""
    with open(_LAUNCH) as f:
        src = f.read()
    assert "amcl_nav2_params.yaml" in src


def test_launch_file_references_odometry_bridge():
    """Launch file must wire in VESC odometry bridge."""
    with open(_LAUNCH) as f:
        src = f.read()
    assert "odometry_bridge.launch.py" in src


def test_launch_file_references_localization():
    """Launch file must include Nav2 localization_launch.py."""
    with open(_LAUNCH) as f:
        src = f.read()
    assert "localization_launch.py" in src


def test_launch_file_references_navigation():
    """Launch file must include Nav2 navigation_launch.py."""
    with open(_LAUNCH) as f:
        src = f.read()
    assert "navigation_launch.py" in src


def test_launch_file_has_map_argument():
    """Launch file must expose 'map' argument for map file path."""
    with open(_LAUNCH) as f:
        src = f.read()
    assert '"map"' in src or "'map'" in src



# ── map_saver_params.yaml + map_persistence files (Issue #696) ───────────────

import os as _os
_PKG_ROOT2 = _os.path.join(_os.path.dirname(__file__), "..")
_MAP_SAVER_CFG  = _os.path.join(_PKG_ROOT2, "config", "map_saver_params.yaml")
_SLAM_LAUNCH    = _os.path.join(_PKG_ROOT2, "launch", "nav2_slam_bringup.launch.py")
_PERSIST_LAUNCH = _os.path.join(_PKG_ROOT2, "launch", "map_persistence.launch.py")
_SAVER_NODE     = _os.path.join(_PKG_ROOT2, "saltybot_nav2_slam", "map_saver_node.py")


def _load_map_saver_params():
    import yaml
    with open(_MAP_SAVER_CFG) as f:
        return yaml.safe_load(f)


def test_map_saver_params_file_exists():
    assert _os.path.isfile(_MAP_SAVER_CFG)


def test_map_saver_params_required_keys():
    p = _load_map_saver_params()["map_saver"]["ros__parameters"]
    for key in ("save_map_timeout", "free_thresh_default",
                "occupied_thresh_default", "map_subscribe_transient_local"):
        assert key in p, f"map_saver_params missing: {key}"


def test_map_saver_thresholds_valid():
    p = _load_map_saver_params()["map_saver"]["ros__parameters"]
    assert 0.0 < p["free_thresh_default"] < p["occupied_thresh_default"] < 1.0


def test_map_saver_timeout_positive():
    p = _load_map_saver_params()["map_saver"]["ros__parameters"]
    assert p["save_map_timeout"] > 0.0


def test_map_persistence_launch_exists():
    assert _os.path.isfile(_PERSIST_LAUNCH)


def test_map_persistence_launch_syntax():
    import py_compile
    py_compile.compile(_PERSIST_LAUNCH, doraise=True)


def test_map_persistence_launch_has_map_saver_server():
    with open(_PERSIST_LAUNCH) as f:
        src = f.read()
    assert "map_saver_server" in src


def test_map_persistence_launch_has_lifecycle_manager():
    with open(_PERSIST_LAUNCH) as f:
        src = f.read()
    assert "lifecycle_manager" in src


def test_map_persistence_launch_has_map_dir_arg():
    with open(_PERSIST_LAUNCH) as f:
        src = f.read()
    assert "map_dir" in src


def test_map_saver_node_exists():
    assert _os.path.isfile(_SAVER_NODE)


def test_map_saver_node_syntax():
    import py_compile
    py_compile.compile(_SAVER_NODE, doraise=True)


def test_map_saver_node_has_save_service():
    with open(_SAVER_NODE) as f:
        src = f.read()
    assert "/saltybot/save_map" in src


def test_map_saver_node_calls_map_saver_cli():
    with open(_SAVER_NODE) as f:
        src = f.read()
    assert "map_saver_cli" in src


def test_map_saver_node_has_auto_save_shutdown():
    with open(_SAVER_NODE) as f:
        src = f.read()
    assert "auto_save_on_shutdown" in src


def test_slam_bringup_includes_map_persistence():
    with open(_SLAM_LAUNCH) as f:
        src = f.read()
    assert "map_persistence.launch.py" in src


def test_slam_bringup_has_map_dir_arg():
    with open(_SLAM_LAUNCH) as f:
        src = f.read()
    assert "map_dir" in src


def test_amcl_launch_has_map_dir_arg():
    import os as __os
    _LAUNCH2 = __os.path.join(__os.path.dirname(__file__), "..", "launch", "nav2_amcl_bringup.launch.py")
    with open(_LAUNCH2) as f:
        src = f.read()
    assert "map_dir" in src


def test_amcl_launch_has_auto_detect_logic():
    import os as __os
    _LAUNCH2 = __os.path.join(__os.path.dirname(__file__), "..", "launch", "nav2_amcl_bringup.launch.py")
    with open(_LAUNCH2) as f:
        src = f.read()
    assert "/mnt/nvme/saltybot/maps" in src
    assert "isfile" in src or "os.path" in src

if __name__ == "__main__":
    pytest.main([__file__, "-v"])
