#!/usr/bin/env python3
"""Measure the fixed-depth floor and derived centreline against the authored grade."""
from __future__ import annotations

import json
import math
from pathlib import Path

ROOT = Path(__file__).resolve().parents[4]
LAYOUT = ROOT / "Saved/Linux/l109/audit-layout-final-stamp.json"
OUT = ROOT / "Saved/Linux/l109/floor-ramp-grade-final.json"
MOUTH_RAMP_METRES = 5.0


def route_m(layout: dict, passage: dict):
    scale = layout["core_voxel_size_um"] / 256.0 / 1_000_000.0
    controls = [(tuple(float(v) * scale for v in row["position_q"]),
                 float(row["radius_q"]) * scale) for row in passage["control_points"]]
    lower_q = passage["lower_mouth_q"]
    player_center_above_floor_q = round(
        1.13 * 256.0 * 1_000_000.0 / layout["core_voxel_size_um"])
    lower_index = next((index for index, row in enumerate(passage["control_points"])
                        if row["position_q"][0] == lower_q[0]
                        and row["position_q"][1] == lower_q[1]
                        and row["position_q"][2] - row["radius_q"]
                            + player_center_above_floor_q == lower_q[2]), None)
    if lower_index is None:
        raise RuntimeError(f"{passage['stable_id']}: lower mouth control is unbound")
    return controls[:lower_index + 1]


def maximum_grade(profile):
    grades = []
    for (p0, z0), (p1, z1) in zip(profile, profile[1:]):
        horizontal = math.hypot(p1[0] - p0[0], p1[1] - p0[1])
        if horizontal > 1.0e-8:
            grades.append(math.degrees(math.atan2(abs(z1 - z0), horizontal)))
    return max(grades, default=0.0)


def main() -> int:
    layout = json.loads(LAYOUT.read_text(encoding="utf-8"))
    rows = []
    for passage in layout["passages"]:
        route = route_m(layout, passage)
        segment_lengths = [math.dist(a[0], b[0]) for a, b in zip(route, route[1:])]
        prefix = [0.0]
        for length in segment_lengths:
            prefix.append(prefix[-1] + length)
        interior_radii = [radius for distance, (_, radius) in zip(prefix, route)
                          if distance >= 10.0 and prefix[-1] - distance >= 10.0]
        minimum_body_radius = min(interior_radii or [radius for _, radius in route])
        body_depth = max(0.5, minimum_body_radius - 0.23)
        scale = layout["core_voxel_size_um"] / 256.0 / 1_000_000.0
        upper_target = max(0.0, route[0][0][2]
                           - passage["upper_mouth_q"][2] * scale + 1.13)
        lower_target = max(0.0, route[-1][0][2]
                           - passage["lower_mouth_q"][2] * scale + 1.13)
        floor_profile = []
        raw_fixed_depth_profile = []
        maximum_centerline_shift = 0.0
        for segment_index, (a, b, length) in enumerate(
                zip(route, route[1:], segment_lengths)):
            steps = max(1, math.ceil(length / 0.05))
            for step in range(steps + 1):
                if floor_profile and step == 0:
                    continue
                t = step / steps
                position = tuple(a[0][axis] + (b[0][axis] - a[0][axis]) * t
                                 for axis in range(3))
                radius = a[1] + (b[1] - a[1]) * t
                along = prefix[segment_index] + length * t
                mouth_distance = max(0.0, min(along, prefix[-1] - along))
                u = max(0.0, min(1.0, mouth_distance / MOUTH_RAMP_METRES))
                ramp = u * u * (3.0 - 2.0 * u)
                local_radius_depth = max(0.0, radius - 0.23)
                mouth_target = upper_target if along <= prefix[-1] * 0.5 else lower_target
                mouth_depth = max(0.0, min(radius, mouth_target))
                route_depth = mouth_depth + (local_radius_depth - mouth_depth) * ramp
                tube_depth = min(body_depth, local_radius_depth)
                effective_center = position[2] - route_depth + tube_depth
                floor_profile.append((position, effective_center - tube_depth))
                raw_fixed_depth_profile.append((position, position[2] - tube_depth))
                maximum_centerline_shift = max(maximum_centerline_shift,
                    abs(effective_center - position[2]))
        rows.append({
            "passage": passage["stable_id"],
            "path_m": prefix[-1],
            "body_floor_depth_m": body_depth,
            "mouth_blend_m": MOUTH_RAMP_METRES,
            "preserved_route_floor_max_grade_deg": maximum_grade(floor_profile),
            "unshifted_fixed_depth_floor_max_grade_deg": maximum_grade(
                raw_fixed_depth_profile),
            "maximum_derived_centerline_shift_m": maximum_centerline_shift,
        })
    OUT.write_text(json.dumps({
        "method": "0.05 m route samples; fixed body depth from the minimum interior radius; "
                  "cubic smoothstep mouth blend",
        "passages": rows,
    }, indent=2) + "\n", encoding="utf-8")
    for row in rows:
        print(f"[L109FloorRamp] {row['passage']} "
              f"depth_m={row['body_floor_depth_m']:.3f} "
              f"preserved_grade_deg={row['preserved_route_floor_max_grade_deg']:.3f} "
              f"unshifted_fixed_depth_grade_deg={row['unshifted_fixed_depth_floor_max_grade_deg']:.3f} "
              f"max_center_shift_m={row['maximum_derived_centerline_shift_m']:.3f}")
    return 0


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