#!/usr/bin/env python3
"""Exact re-implementation of the engine's map floor test (FUN_004036d0, floor branch) on raw collision records.

  floortest.py LEVEL.wad X Y Z        world position in cells (game space, z = -A.z); prints every polygon of the
                                      cell's tiles that is a floor candidate, whether it passes, and the winner

Integer arithmetic as in groove.exe (20.12 fixed point, sine table read from re/groove.exe, C division).
See notes/geometry.md "Floor test: inclusion rule and the floating-next-to-the-log case".
"""
import os
import struct
import sys

HERE = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, HERE)
import geomlib as G  # noqa: E402

_EXE = os.path.join(os.path.dirname(HERE), 're', 'groove.exe')
SIN = struct.unpack_from('<4096i', open(_EXE, 'rb').read(), 0x574318 - 0x400000)  # DAT_00574318
PROBE = 0x400  # DAT_0057dd98 (0.25) unless strat flag 0x200000


def _c8(b):
    return b - 256 if b > 127 else b


def _cdiv(a, b):
    q = abs(a) // abs(b)
    return q if (a >= 0) == (b > 0) else -q


def to_local(px, pz, tile_b):
    """World (fixed) -> tile-local x/z exactly like 0x4037b3..0x4037f1 (rotation by -ry)."""
    lx, lz = px - tile_b[3], pz - tile_b[5]
    u = (-tile_b[1]) & 0xffffffff
    if u:
        s, c = SIN[u & 0xfff], SIN[(u + 0x400) & 0xfff]
        lx, lz = (s * lz + c * lx) >> 12, (c * lz - s * lx) >> 12
    return lx, lz


def test_poly(r, lx, ly, lz):
    """One 32-byte record. ly = probe height (feet - tile y + 0.25). Returns None (skipped) or
    (dist, plane_y_local, passed, edge_values, tolerance); dist = probe height above the plane."""
    ny32 = _c8(r[3]) * 32
    if ny32 <= 0:  # only up-facing planes (0x200000 strats also take down-facing ones)
        return None
    d0 = struct.unpack_from('<h', r, 6)[0]
    i6 = _cdiv(d0 * 4096 - _c8(r[4]) * 32 * lz - _c8(r[2]) * 32 * lx - ny32 * ly, ny32)
    dist = -i6
    y = ly + i6  # the probe dropped vertically onto the plane
    tol = -0x40 - ((0x1000 - ny32) >> 6)
    vals = []
    for k in range(4 if r[0] & 1 else 3):
        nx, ny, nz, _, d = struct.unpack_from('<bbbBh', r, 8 + 6 * k)
        vals.append(((ny * 32 * y + nz * 32 * lz + nx * 32 * lx) >> 12) - d)
    return dist, y, all(v >= tol for v in vals), vals, tol


def floor(wad, x, y, z, wfpc_8000000=False):
    ch, wf, trak, mp = G.load_level(wad)
    px, py, pz = (int(round(v * 4096)) for v in (x, y, z))
    skip_mat = 0x12 if wfpc_8000000 else 0x11
    best = None
    rows = []
    for t in range(mp['N']):
        b = mp['B'][t]
        if (b[3] >> 12, b[5] >> 12) != (px >> 12, pz >> 12):
            continue
        lx, lz = to_local(px, pz, b)
        ly = py - b[4] + PROBE
        recs = [r for g in trak[mp['midx'][t]].coll for r in g]
        for i, r in enumerate(recs):
            if r[1] == skip_mat:
                continue
            res = test_poly(r, lx, ly, lz)
            if res is None or res[0] < -0x80:
                continue  # plane more than 1/32 above the probe: ceiling branch, not a floor
            dist, yl, ok, vals, tol = res
            rows.append((t, i, r[1], dist / 4096, (b[4] + yl) / 4096, ok, vals, tol))
            if ok and (best is None or dist < best[0]):
                best = (dist, t, i, (b[4] + yl) / 4096)
    return rows, best


if __name__ == '__main__':
    if len(sys.argv) != 5:
        sys.exit(__doc__)
    rows, best = floor(sys.argv[1], *map(float, sys.argv[2:5]))
    for t, i, mat, dist, h, ok, vals, tol in rows:
        print(f'tile {t} poly {i:2d} mat {mat:3d}  probe-plane {dist:+.4f}  plane y {h:.4f}  '
              f'{"PASS" if ok else "fail"}  edges {vals} (tolerance {tol})')
    print('floor:', f'tile {best[1]} poly {best[2]} height {best[3]:.4f}' if best else 'none in this cell')