#!/usr/bin/env python3
"""Trace review-image pixels through the exported clipmap mesh."""
import array
import math
import struct
import sys


def read_exact(f, count):
    data = f.read(count)
    if len(data) != count:
        raise EOFError((len(data), count))
    return data


def floor_div(n, d):
    q, r = divmod(n, d)
    return q - (r < 0)


def visible_mask(key, desired):
    x, y, z, lod = key
    extent = 32 << lod
    origin = (x * extent, y * extent, z * extent)
    mask = 0
    for axis in range(3):
        for side in range(2):
            probe = [origin[i] + extent // 2 for i in range(3)]
            probe[axis] = origin[axis] + (extent if side else -1)
            neighbor = None
            for level in range(9):
                neighbor_extent = 32 << level
                coord = tuple(floor_div(probe[i], neighbor_extent) for i in range(3)) + (level,)
                if coord in desired:
                    neighbor = level
                    break
            if neighbor is None or neighbor != lod:
                mask |= 1 << (axis * 2 + side)
    return mask


def ray_box(origin, direction, low, high):
    lo, hi = 0.1, 20000.0
    for axis in range(3):
        if abs(direction[axis]) < 1e-12:
            if origin[axis] < low[axis] or origin[axis] > high[axis]:
                return None
        else:
            a = (low[axis] - origin[axis]) / direction[axis]
            b = (high[axis] - origin[axis]) / direction[axis]
            if a > b:
                a, b = b, a
            lo, hi = max(lo, a), min(hi, b)
            if lo > hi:
                return None
    return lo, hi


def ray_tri(origin, direction, a, b, c):
    e1 = tuple(b[i] - a[i] for i in range(3))
    e2 = tuple(c[i] - a[i] for i in range(3))
    p = (direction[1]*e2[2]-direction[2]*e2[1],
         direction[2]*e2[0]-direction[0]*e2[2],
         direction[0]*e2[1]-direction[1]*e2[0])
    det = sum(e1[i]*p[i] for i in range(3))
    if abs(det) < 1e-10:
        return None
    inv = 1.0/det
    tvec = tuple(origin[i]-a[i] for i in range(3))
    u = sum(tvec[i]*p[i] for i in range(3))*inv
    if u < 0 or u > 1:
        return None
    q = (tvec[1]*e1[2]-tvec[2]*e1[1],
         tvec[2]*e1[0]-tvec[0]*e1[2],
         tvec[0]*e1[1]-tvec[1]*e1[0])
    v = sum(direction[i]*q[i] for i in range(3))*inv
    if v < 0 or u+v > 1:
        return None
    t = sum(e2[i]*q[i] for i in range(3))*inv
    return t if t >= 0.1 else None


if len(sys.argv) < 8 or len(sys.argv) % 2 != 1:
    raise SystemExit("usage: ProbeCaptureRays.py scene.vfmesh camX camY camZ yaw pitch pixelX pixelY [pixelX pixelY ...]")
scene_path = sys.argv[1]
cam = tuple(map(float, sys.argv[2:5]))
yaw, pitch = map(math.radians, map(float, sys.argv[5:7]))
pixels = [(int(sys.argv[i]), int(sys.argv[i+1])) for i in range(7, len(sys.argv), 2)]
width, height, fov = 1600, 900, math.radians(90)
tan_h = math.tan(fov/2)
tan_v = tan_h / (width/height)
forward = (math.sin(yaw)*math.cos(pitch), math.cos(yaw)*math.cos(pitch), math.sin(pitch))
right = (math.cos(yaw), -math.sin(yaw), 0.0)
up = (right[1]*forward[2]-right[2]*forward[1], right[2]*forward[0]-right[0]*forward[2], right[0]*forward[1]-right[1]*forward[0])

with open(scene_path, "rb") as f:
    if read_exact(f, 8) != b"VFMSH001":
        raise SystemExit("bad scene")
    version, voxel_um, count = struct.unpack("<3I", read_exact(f, 12))
    scene_cam = struct.unpack("<3d", read_exact(f, 24))
    scene_yaw, scene_pitch = struct.unpack("<2d", read_exact(f, 16))
    voxel_m = voxel_um / 1_000_000.0
    tiles = []
    desired = set()
    for _ in range(count):
        key = struct.unpack("<4i", read_exact(f, 16))
        surface_vertices, skirt_count, vertex_count, index_count, mask_count = struct.unpack("<5I", read_exact(f, 20))
        positions = array.array("f"); positions.frombytes(read_exact(f, vertex_count*12))
        normals = array.array("f"); normals.frombytes(read_exact(f, vertex_count*12))
        indices = array.array("I"); indices.frombytes(read_exact(f, index_count*4))
        masks = read_exact(f, mask_count)
        if sys.byteorder != "little":
            positions.byteswap(); normals.byteswap(); indices.byteswap()
        desired.add(key)
        tiles.append((key, surface_vertices, skirt_count, positions, normals, indices, masks))

print(f"scene={scene_path} tiles={count} camera={cam} yaw={math.degrees(yaw):.3f} pitch={math.degrees(pitch):.3f}")
for px, py in pixels:
    nx = 2*px/width - 1
    ny = 1 - 2*py/height
    direction0 = tuple(forward[i] + nx*tan_h*right[i] + ny*tan_v*up[i] for i in range(3))
    length = math.sqrt(sum(v*v for v in direction0))
    direction = tuple(v/length for v in direction0)
    hits = []
    candidate_tiles = 0
    for key, surface_vertices, skirt_count, positions, normals, indices, masks in tiles:
        x,y,z,lod = key
        extent = 32*(1<<lod)*voxel_m
        low = (x*extent, y*extent, z*extent)
        high = tuple(v+extent for v in low)
        if ray_box(cam, direction, low, high) is None:
            continue
        candidate_tiles += 1
        show_mask = visible_mask(key, desired)
        for tri in range(len(indices)//3):
            m = masks[tri] if masks else 0
            if m and (m & show_mask) == 0:
                continue
            ia,ib,ic = indices[tri*3:tri*3+3]
            a,b,c = [],[],[]
            for idx, out in ((ia,a),(ib,b),(ic,c)):
                base=idx*3
                out.extend((positions[base],positions[base+1],positions[base+2]))
            t=ray_tri(cam,direction,a,b,c)
            if t is None: continue
            p=tuple(cam[i]+t*direction[i] for i in range(3))
            nz=(normals[ia*3+2]+normals[ib*3+2]+normals[ic*3+2])/3
            hits.append((t,key,tri,m,p,nz,ia>=surface_vertices or ib>=surface_vertices or ic>=surface_vertices))
    hits.sort(key=lambda h:h[0])
    print(f"pixel=({px},{py}) ray={direction} candidate_tiles={candidate_tiles} hits={len(hits)}")
    for t,key,tri,m,p,nz,skirt in hits[:12]:
        print(f"  t={t:.3f}m key={key} tri={tri} mask={m} skirt={skirt} z={p[2]:.3f} normal_z={nz:.3f} xyz=({p[0]:.3f},{p[1]:.3f},{p[2]:.3f})")