#!/usr/bin/env python3
"""Evaluate navigation test summaries against explicit acceptance thresholds."""

import argparse
import glob
import json
from pathlib import Path
import sys


def nested_value(data, *keys):
    value = data
    for key in keys:
        if not isinstance(value, dict) or key not in value:
            return None
        value = value[key]
    return value


def check_max(failures, label, value, limit):
    if value is None:
        failures.append(f"{label}: missing")
    elif value > limit:
        failures.append(f"{label}: {value:.4f} > {limit:.4f}")


def check_min(failures, label, value, limit):
    if value is None:
        failures.append(f"{label}: missing")
    elif value < limit:
        failures.append(f"{label}: {value:.4f} < {limit:.4f}")


def evaluate_planning(data, args):
    failures = []
    ok_count = int(data.get("plan_ok_status_count", 0))
    fail_count = int(data.get("plan_fail_status_count", 0))
    attempts = ok_count + fail_count
    success_rate = ok_count / attempts if attempts else None
    check_min(failures, "planning success rate", success_rate, args.min_plan_success)
    check_max(
        failures,
        "planning latency p95 (s)",
        nested_value(data, "planning_latency_sec", "p95"),
        args.max_planning_p95,
    )
    check_max(
        failures,
        "occupied path points",
        data.get("path_occupied_points_max"),
        args.max_occupied_points,
    )
    minimum_radius = data.get("minimum_planned_turning_radius_m")
    if minimum_radius is not None:
        check_min(
            failures,
            "minimum turning radius (m)",
            minimum_radius,
            args.min_turning_radius,
        )
    elif int(data.get("plan_count", 0)) <= 0:
        failures.append("minimum turning radius (m): missing because no path was recorded")
    return failures


def evaluate_tracking(data, args):
    failures = []
    check_max(
        failures,
        "cross-track RMS (m)",
        nested_value(data, "cross_track_error_m", "rms"),
        args.max_cross_track_rms,
    )
    check_max(
        failures,
        "cross-track p95 (m)",
        nested_value(data, "cross_track_error_m", "p95"),
        args.max_cross_track_p95,
    )
    check_max(
        failures,
        "cross-track max (m)",
        nested_value(data, "cross_track_error_m", "max"),
        args.max_cross_track,
    )
    check_max(
        failures,
        "final goal error (m)",
        data.get("final_goal_error_m"),
        args.max_goal_error,
    )
    check_max(
        failures,
        "cancel-to-zero p95 (s)",
        nested_value(data, "cancel_to_zero_command_sec", "p95"),
        args.max_cancel_latency,
    )
    check_max(
        failures,
        "Ackermann drive speed max (m/s)",
        data.get("max_abs_drive_speed"),
        args.max_drive_speed,
    )
    check_max(
        failures,
        "Ackermann steering max (rad)",
        data.get("max_abs_steering_angle"),
        args.max_steering_angle,
    )
    transform_failures = int(data.get("transform_failures", 0))
    if transform_failures > args.max_transform_failures:
        failures.append(
            f"transform failures: {transform_failures} > {args.max_transform_failures}"
        )
    return failures


def resolve_summary_paths(values):
    paths = []
    unmatched = []
    for value in values:
        expanded_value = str(Path(value).expanduser())
        matches = sorted(glob.glob(expanded_value))
        if not matches:
            unmatched.append(value)
            continue
        paths.extend(Path(match) for match in matches if Path(match).is_file())
    return paths, unmatched


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("mode", choices=("planning", "tracking"))
    parser.add_argument("summaries", nargs="+")
    parser.add_argument("--min-plan-success", type=float, default=0.95)
    parser.add_argument("--max-planning-p95", type=float, default=0.50)
    parser.add_argument("--max-occupied-points", type=int, default=0)
    parser.add_argument("--min-turning-radius", type=float, default=0.36)
    parser.add_argument("--max-cross-track-rms", type=float, default=0.12)
    parser.add_argument("--max-cross-track-p95", type=float, default=0.20)
    parser.add_argument("--max-cross-track", type=float, default=0.30)
    parser.add_argument("--max-goal-error", type=float, default=0.15)
    parser.add_argument("--max-cancel-latency", type=float, default=0.25)
    parser.add_argument("--max-drive-speed", type=float, default=0.21)
    parser.add_argument("--max-steering-angle", type=float, default=0.601)
    parser.add_argument("--max-transform-failures", type=int, default=0)
    args = parser.parse_args()

    summary_paths, unmatched = resolve_summary_paths(args.summaries)
    if unmatched:
        for pattern in unmatched:
            print(f"ERROR no summary files matched: {pattern}", file=sys.stderr)
    if not summary_paths:
        return 2

    overall_success = True
    for summary_path in summary_paths:
        try:
            data = json.loads(summary_path.read_text(encoding="utf-8"))
        except (OSError, json.JSONDecodeError) as exc:
            overall_success = False
            print(f"FAIL {summary_path}")
            print(f"  - unable to read valid JSON: {exc}")
            continue
        if args.mode == "planning":
            failures = evaluate_planning(data, args)
        else:
            failures = evaluate_tracking(data, args)
        if failures:
            overall_success = False
            print(f"FAIL {summary_path}")
            for failure in failures:
                print(f"  - {failure}")
        else:
            print(f"PASS {summary_path}")
    return 0 if overall_success and not unmatched else 1


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