"""Offline authoring check for a stable, metre-wide start on a scan proxy.

Uses world geometry only to prepare a scene. No candidate map enters navigation.
An operator/engineer still verifies the chosen start against the visual trail.
"""

import argparse
import json
import math
import sys
from pathlib import Path

import numpy as np

sys.path.insert(0, str(Path(__file__).resolve().parents[1]))
from spawn_clearance import obstructing_triangles
from terrain import load_glb


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--terrain", type=Path, required=True)
    args = parser.parse_args()
    manifest = json.loads((args.terrain / "terrain.json").read_text(encoding="utf-8-sig"))
    settings = manifest["settings"]
    points, indices = load_glb(args.terrain / "terrain.collision.glb")
    triangles = points[indices]
    low, high = triangles.min(axis=1), triangles.max(axis=1)
    center = np.array(settings["spawn_xy"])
    selected = (
        (low[:, :2] <= center + 2.5).all(axis=1)
        & (high[:, :2] >= center - 2.5).all(axis=1)
        & (low[:, 2] < settings["ground_z"] + 1.5)
        & (high[:, 2] > settings["ground_z"] - 0.8)
    )
    triangles, low, high = triangles[selected], low[selected], high[selected]

    def heights(x, y):
        hits = triangles[
            (low[:, 0] <= x) & (high[:, 0] >= x) & (low[:, 1] <= y) & (high[:, 1] >= y)
        ]
        if not len(hits):
            return np.empty(0)
        a, b, c = hits[:, 0], hits[:, 1], hits[:, 2]
        den = (b[:, 1] - c[:, 1]) * (a[:, 0] - c[:, 0]) + (c[:, 0] - b[:, 0]) * (a[:, 1] - c[:, 1])
        valid = np.abs(den) > 1e-8
        a, b, c, den = a[valid], b[valid], c[valid], den[valid]
        u = ((b[:, 1] - c[:, 1]) * (x - c[:, 0]) + (c[:, 0] - b[:, 0]) * (y - c[:, 1])) / den
        v = ((c[:, 1] - a[:, 1]) * (x - c[:, 0]) + (a[:, 0] - c[:, 0]) * (y - c[:, 1])) / den
        inside = (u >= -1e-6) & (v >= -1e-6) & (u + v <= 1 + 1e-6)
        return (u * a[:, 2] + v * b[:, 2] + (1 - u - v) * c[:, 2])[inside]

    angle = math.radians(settings["heading_degrees"])
    rotation = np.array([[math.cos(angle), -math.sin(angle)], [math.sin(angle), math.cos(angle)]])
    footprint = np.array([[x, y] for x in (-0.5, 0, 0.5) for y in (-0.5, 0, 0.5)]) @ rotation.T
    candidates = []
    for dx in np.arange(-2, 2.01, 0.2):
        for dy in np.arange(-2, 2.01, 0.2):
            position = center + [dx, dy]
            support = []
            for x, y in footprint + position:
                z = heights(x, y)
                near = z[np.abs(z - settings["ground_z"]) < 0.8]
                if not len(near):
                    break
                ground = near.max()
                if np.any((z > ground + 0.12) & (z < ground + 1)):
                    break
                support.append(float(ground))
            if len(support) != 9:
                continue
            design = np.column_stack((footprint, np.ones(9)))
            plane = np.linalg.lstsq(design, np.asarray(support), rcond=None)[0]
            residual = float(np.max(np.abs(design @ plane - support)))
            slope = math.degrees(math.atan(np.linalg.norm(plane[:2])))
            if residual > 0.08 or slope > 20:
                continue
            if obstructing_triangles(
                points, indices, position, settings["heading_degrees"], plane
            ):
                continue
            candidates.append(
                {
                    "xy": position.tolist(),
                    "ground_z": float(np.median(support)),
                    "height_span": max(support) - min(support),
                    "offset_m": math.hypot(dx, dy),
                    "residual_m": residual,
                    "slope_degrees": slope,
                }
            )
    candidates.sort(key=lambda row: row["offset_m"] + 2 * row["height_span"])
    print(
        json.dumps(
            {
                "world_sha256": manifest["source_sha256"],
                "candidate_count": len(candidates),
                "candidates": candidates[:12],
            }
        )
    )


if __name__ == "__main__":
    main()