#!/usr/bin/env python3
"""Measure surface heights and normals on an eastward FI floor radial line."""
import array
import math
import struct
import sys


def read_surface_vertices(path):
    vertices = []
    with open(path, "rb") as stream:
        if stream.read(8) != b"VFMSH001":
            raise SystemExit(f"{path}: invalid scene header")
        version, voxel_um, count = struct.unpack("<3I", stream.read(12))
        stream.read(40)
        if version != 1 or count > 100000:
            raise SystemExit("unsupported scene")
        for _ in range(count):
            key = struct.unpack("<4i", stream.read(16))
            surface_count, skirt_count, vertex_count, index_count, mask_count = struct.unpack(
                "<5I", stream.read(20))
            positions = array.array("f")
            positions.frombytes(stream.read(vertex_count * 12))
            normals = array.array("f")
            normals.frombytes(stream.read(vertex_count * 12))
            indices = array.array("I")
            indices.frombytes(stream.read(index_count * 4))
            masks = stream.read(mask_count)
            if sys.byteorder != "little":
                positions.byteswap()
                normals.byteswap()
                indices.byteswap()
            used = set()
            for tri in range(0, index_count, 3):
                if masks and masks[tri // 3]:
                    continue
                for index in indices[tri:tri + 3]:
                    if index < surface_count:
                        used.add(index)
            for index in used:
                offset = index * 3
                vertices.append((positions[offset], positions[offset + 1],
                                 positions[offset + 2], normals[offset],
                                 normals[offset + 1], normals[offset + 2]))
    return voxel_um, vertices


def main():
    scene = sys.argv[1]
    center_x, center_y = 64.0, -32.0
    voxel_um, vertices = read_surface_vertices(scene)
    floor = [v for v in vertices if abs(v[2] + 119.5) < 0.05
             and math.hypot(v[0] - center_x, v[1] - center_y) <= 16.5]
    if not floor:
        raise SystemExit("no floor surface vertices near expected FI bedrock top")
    mean = tuple(sum(v[index] for v in floor) / len(floor) for index in (3, 4, 5))
    normal_rms = math.sqrt(sum(sum((v[index] - mean[axis]) ** 2
                                   for axis, index in enumerate((3, 4, 5)))
                               for v in floor) / len(floor))
    min_z = min(v[2] for v in floor)
    max_z = max(v[2] for v in floor)
    print(f"[RingNormalField] voxel_um={voxel_um} center_m=({center_x:.3f},{center_y:.3f}) "
          f"vertices={len(floor)} height_min_m={min_z:.6f} height_max_m={max_z:.6f} "
          f"mean_normal=({mean[0]:.8f},{mean[1]:.8f},{mean[2]:.8f}) "
          f"normal_rms={normal_rms:.9f}")
    print("[RingNormalRadialLine] radius_m sampled_radius_m height_m nx ny nz")
    for radius in range(1, 17):
        x, y = center_x + radius, center_y
        vertex = min(floor, key=lambda v: (v[0] - x) ** 2 + (v[1] - y) ** 2)
        sampled_radius = math.hypot(vertex[0] - center_x, vertex[1] - center_y)
        if math.hypot(vertex[0] - x, vertex[1] - y) > 0.5:
            raise SystemExit(f"missing near vertex at radius {radius} m")
        print(f"{radius:2d} {sampled_radius:.6f} {vertex[2]:.6f} "
              f"{vertex[3]:.8f} {vertex[4]:.8f} {vertex[5]:.8f}")


if __name__ == "__main__":
    main()