"""Bounded synthetic acceptance of the installed CMU image on Worker only.

This tests navigation independently of perception and physics. It does not
claim Gaussian-world or physical-rover acceptance. Owns one temporary container.
"""

import argparse
import http.client
import json
import math
import subprocess
import time
from datetime import UTC, datetime
from pathlib import Path
from uuid import uuid4


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--adapter", type=Path, required=True)
    parser.add_argument("--output", type=Path, required=True)
    args = parser.parse_args()
    profile = json.loads((args.adapter / "models.worker-006.json").read_text())
    name = "ndc-mission-core-ai-module-navigation-check-" + uuid4().hex[:8]
    identity = subprocess.check_output(
        [
            "docker",
            "run",
            "-d",
            "--name",
            name,
            "--cpus",
            "3",
            "--memory",
            "2g",
            "--label",
            "com.nodedc.stack=ai-polygon-qualification",
            "--label",
            "com.nodedc.product=mission-core",
            "--label",
            "com.nodedc.role=qualification",
            "--label",
            "com.nodedc.managed-by=ai-polygon-qualification",
            "-p",
            "127.0.0.1:18193:8010",
            "--mount",
            f"type=bind,source={args.adapter},target=/adapter,readonly",
            profile["navigation"]["image"],
        ],
        text=True,
    ).strip()
    connection = http.client.HTTPConnection("127.0.0.1", 18193, timeout=3)

    def request(path, body=None):
        connection.request(
            "GET" if body is None else "POST", path, body=None if body is None else json.dumps(body)
        )
        response = connection.getresponse()
        value = json.loads(response.read())
        if response.status != 200:
            raise RuntimeError(str(value))
        return value

    def ready():
        deadline = time.monotonic() + 15
        while time.monotonic() < deadline:
            try:
                request("/ready")
                return
            except (OSError, RuntimeError, http.client.HTTPException):
                time.sleep(0.2)
        raise TimeoutError("CMU qualification container did not become ready")

    report = {
        "image": profile["navigation"]["image"],
        "cases": [],
        "utc": datetime.now(UTC).isoformat(),
        "monotonic": time.monotonic(),
    }
    try:
        ready()
        floor = [[x / 10, y / 10, 0.0] for x in range(-20, 41) for y in range(-20, 21)]
        wall = [[1.5, y / 10, z / 10] for y in range(-20, 21) for z in range(1, 16)]
        enclosure = [
            [side * 0.9, y / 10, z / 10]
            for side in (-1, 1)
            for y in range(-9, 10)
            for z in range(1, 16)
        ]
        enclosure += [
            [x / 10, side * 0.9, z / 10]
            for side in (-1, 1)
            for x in range(-9, 10)
            for z in range(1, 16)
        ]

        def corridor(half_width):
            return floor + [
                [x / 10, side * half_width, z / 10]
                for x in range(-15, 51)
                for side in (-1, 1)
                for z in range(1, 16)
            ]

        for name, points in (
            ("clear-flat-ground", floor),
            ("rear-reserve-forward-escape", floor + [[-0.53, 0, z / 10] for z in range(1, 10)]),
            ("front-reserve-stop", floor + [[0.53, 0, z / 10] for z in range(1, 10)]),
            ("body-overlap-stop", floor + [[-0.49, 0, z / 10] for z in range(1, 10)]),
            ("reverse-clear-ground", floor),
            (
                "reverse-blocked-by-rear-wall",
                floor + [[-0.7, y / 10, z / 10] for y in range(-10, 11) for z in range(1, 12)],
            ),
            ("turn-away-from-wall", floor + wall),
            (
                "close-wall-stop",
                floor + [[0.65, y / 10, z / 10] for y in range(-20, 21) for z in range(1, 16)],
            ),
            ("closed-enclosure-stop", floor + enclosure),
            ("metre-rover-in-1.3m-corridor", corridor(0.65)),
            ("metre-rover-rejects-0.9m-corridor", corridor(0.45)),
            (
                "rear-corner-selects-clear-primitive",
                floor + [[-0.62, -0.39, z / 10] for z in range(1, 15)],
            ),
        ):
            request("/reset", {})
            ready()
            samples = []
            for _ in range(20):
                samples.append(
                    request(
                        "/plan",
                        {
                            "points": points,
                            "pose": [0, 0, 0.27, 0, 0, 0, 1],
                            "goal": [-0.65, 0, 0]
                            if name.startswith("reverse-")
                            else ([2, -2, 0] if name.startswith("rear-corner") else [3, 0, 0]),
                            "allow_reverse": name.startswith("reverse-"),
                            "max_speed_mps": 0.15,
                        },
                    )
                )
                time.sleep(0.2)
            if name in (
                "clear-flat-ground",
                "metre-rover-in-1.3m-corridor",
                "rear-reserve-forward-escape",
            ):
                passed = any(v["speed_mps"] > 0.05 and v["status"] == "path" for v in samples)
            elif name == "reverse-clear-ground":
                passed = any(v["speed_mps"] < -0.05 and v["status"] == "path" for v in samples[-5:])
            elif name == "turn-away-from-wall":
                valid = [v for v in samples[-5:] if v["status"] == "path"]
                passed = len(valid) >= 3 and all(
                    (v["speed_mps"] > 0.02 or abs(v["yaw_rate_rps"]) > 0.1)
                    and max(p[0] for p in v["path"]) < 0.75
                    for v in valid
                )
            elif name.startswith("rear-corner"):
                passed = any(v["speed_mps"] > 0.05 for v in samples[-5:]) and all(
                    v.get("diagnostic", {}).get("failure") != "footprint" for v in samples[-5:]
                )
            else:
                passed = all(v["speed_mps"] == 0 and v["status"] == "blocked" for v in samples[-5:])
            report["cases"].append({"name": name, "passed": passed, "samples": samples})
        # A vanished return is occlusion, not proof of free space. Observe a
        # near wall, then only distant ground for longer than the decay timer.
        request("/reset", {})
        ready()
        close_wall = [[0.7, y / 10, z / 10] for y in range(-10, 11) for z in range(1, 10)]
        samples = []
        for i in range(30):
            samples.append(
                request(
                    "/plan",
                    {
                        "points": floor + close_wall if i < 5 else [p for p in floor if p[0] > 1.2],
                        "pose": [0, 0, 0.37, 0, 0, 0, 1],
                        "goal": [3, 0, 0],
                        "max_speed_mps": 0.15,
                    },
                )
            )
            time.sleep(0.2)
        report["cases"].append(
            {
                "name": "occluded-near-obstacle-retained",
                "samples": samples,
                "passed": all(
                    x["speed_mps"] == 0 and x["status"] == "blocked" for x in samples[-5:]
                ),
            }
        )
        # Navigation must admit the same continuous grades as the measured
        # physical profile, while retaining a discontinuous 15 cm ledge.
        for angle, quantized in [(a, False) for a in (5, 10, 15, 20, 25)] + [
            (10, True),
            (20, True),
        ]:
            request("/reset", {})
            ready()
            radians = math.radians(angle)
            slope = math.tan(radians)
            samples = []
            for _ in range(12):
                samples.append(
                    request(
                        "/plan",
                        {
                            "points": [
                                [x, y, round(x * slope / 0.06) * 0.06 if quantized else x * slope]
                                for x, y, _ in floor
                            ],
                            "pose": [
                                0,
                                0,
                                0.37,
                                0,
                                -math.sin(radians / 2),
                                0,
                                math.cos(radians / 2),
                            ],
                            "goal": [3, 0, 3 * slope],
                            "max_speed_mps": 0.15,
                        },
                    )
                )
                time.sleep(0.2)
            report["cases"].append(
                {
                    "name": f"supported-{'voxel-' if quantized else ''}ramp-{angle}-degrees",
                    "samples": samples,
                    "passed": any(s["speed_mps"] > 0.05 for s in samples[-5:]),
                }
            )
        for height in (0.12, 0.15, -0.4):
            request("/reset", {})
            ready()
            points = [[x, y, height if x >= 0.7 else 0] for x, y, _ in floor]
            points += [
                [x / 10, side * 0.65, z / 10]
                for x in range(-15, 41)
                for side in (-1, 1)
                for z in range(1, 16)
            ]
            samples = []
            for _ in range(12):
                samples.append(
                    request(
                        "/plan",
                        {
                            "points": points,
                            "pose": [0, 0, 0.37, 0, 0, 0, 1],
                            "goal": [3, 0, 0],
                            "max_speed_mps": 0.15,
                        },
                    )
                )
                time.sleep(0.2)
            report["cases"].append(
                {
                    "name": f"discontinuous-height-{height:+.2f}m-stop",
                    "samples": samples,
                    "passed": all(
                        s["speed_mps"] == 0 and s["status"] == "blocked" for s in samples[-5:]
                    ),
                }
            )
        report["passed"] = all(case["passed"] for case in report["cases"])
    finally:
        report["container_log_tail"] = subprocess.run(
            ["docker", "logs", "--tail", "30", identity], capture_output=True, text=True
        ).stderr
        args.output.parent.mkdir(parents=True, exist_ok=True)
        args.output.write_text(json.dumps(report, indent=2), encoding="utf-8")
        connection.close()
        subprocess.run(["docker", "rm", "-f", identity], check=True, capture_output=True)
    print(
        json.dumps(
            {
                "passed": report["passed"],
                "cases": [
                    {"name": row["name"], "passed": row["passed"]} for row in report["cases"]
                ],
            }
        )
    )


if __name__ == "__main__":
    main()