"""RETINA — омматидиальная решётка.

Фасеточный глаз дрозофилы — регулярная решётка омматидиев, каждый смотрит в свою
фиксированную сторону. Вращающийся лидар устроен так же: пара (кольцо, столбец)
задаёт направление луча. Поэтому облако точек сразу переводится в *ретинотопический*
дальностный образ `(кольцо, азимут)`, и вся дальнейшая обработка идёт в этой
решётке — как в зрительной системе мухи, а не в неупорядоченном облаке.

Три особенности конкретного сенсора, измеренные по данным (см. `docs/ALGORITHM.md`):

1. **Раскладка различается между бэгами**: 3600 азимутов на 360° против 1200 на
   100°. Решётка калибруется по самим данным, ничего не захардкожено.
2. **Двойное эхо**: соседние столбцы делят один азимут. Когда эхо одно, оба слота
   содержат одно значение; когда два — ближнее несёт объект, дальнее фон за ним.
   Реально различаются ~3 % лучей, и это именно тонкие предметы и кромки.
3. **Скос решётки**: у каждого лазерного канала свой постоянный азимутальный сдвиг,
   разброс достигает **15.5°** (≈155 столбцов). В сыром виде «столбец» не является
   направлением: соседние кольца одного столбца смотрят в стороны, разнесённые на
   градусы. Поэтому образ **выпрямляется** целочисленным сдвигом строк; остаточная
   ошибка < половины шага азимута и учитывается в таблице направлений.

Быстрый путь опирается на порядок точек (столбец · эхо · кольцо). В синтетическом
бэге организаторов он соблюдается не везде: у облака нет поля `ring`, а там, где
вставлен предмет, заслонённые им точки удалены, а точки предмета вписаны в
середину массива — всё, что дальше, сдвинуто (так в половине кадров). Поэтому
порядок проверяется в каждом кадре: у кадра с целым порядком элевация каждой
точки совпадает с элевацией её кольца до 0.0001°. Кадр, где это не так,
раскладывается в ту же решётку **по углам каждой точки** — медленнее, зато без
допущений о порядке. Калибровка без поля `ring` находит кольца по гистограмме
элевации и берёт только кадры с целым порядком, а если таких нет — строит
решётку целиком по углам.
"""
from __future__ import annotations

import warnings
from contextlib import contextmanager
from dataclasses import dataclass
from pathlib import Path

import numpy as np

from .cdr import PointCloud2

DEG = np.pi / 180.0

# Допуск проверки порядка точек: в целом кадре элевация точки совпадает с
# элевацией её кольца до 0.0001° (замерено на всех бэгах и на синтетике), а
# ближайшие кольца Pandar128 разнесены на 0.086°. Вставленные точки предмета
# уходят от колец на 0.03° и больше, сдвинутые — на целое кольцо.
ORDER_TOL_DEG = 0.01


@dataclass
class RangeImage:
    """Выпрямленный дальностный образ в координатах `(кольцо, азимут)`."""

    stamp: float
    r_near: np.ndarray   # (H, W) float32 — ближнее эхо, 0 = нет эха
    r_far: np.ndarray    # (H, W) float32 — дальнее эхо, 0 = нет эха
    inten: np.ndarray    # (H, W) float32 — интенсивность ближнего эха
    valid: np.ndarray    # (H, W) bool

    @property
    def shape(self) -> tuple[int, int]:
        return self.r_near.shape

    def xyz(self, layout: "ScanLayout") -> np.ndarray:
        """Декартовы координаты ближнего эха: (H, W, 3)."""
        return layout.dirs * self.r_near[..., None]

    def crop(self, cols: slice) -> "RangeImage":
        return RangeImage(self.stamp, self.r_near[:, cols], self.r_far[:, cols],
                          self.inten[:, cols], self.valid[:, cols])


class ScanLayout:
    """Калиброванная и выпрямленная решётка лучей.

    Азимут отсчитывается от направления движения (вперёд = −Y в системе сенсора),
    положительный вправо; элевация — вверх от горизонтали сенсора.
    """

    def __init__(self, el_deg: np.ndarray, az_step_deg: float, az0_deg: float,
                 col_shift: np.ndarray, az_resid_deg: np.ndarray,
                 n_az: int, n_echo: int, wrap: bool = False, indexed: bool = True):
        self.el_deg = np.asarray(el_deg, np.float64)            # (H,)
        self.az_step_deg = float(az_step_deg)                   # шаг на азимутальный индекс
        self.az0_deg = float(az0_deg)
        self.col_shift = np.asarray(col_shift, np.int64)        # (H,) выпрямление
        self.az_resid_deg = np.asarray(az_resid_deg, np.float64)  # (H,) остаток < шага/2
        self.n_az = int(n_az)
        self.n_echo = int(n_echo)
        self.wrap = bool(wrap)          # круговой скан: края смыкаются
        # False — порядок точек неизвестен (решётка построена по углам), и
        # каждый кадр раскладывается по углам точек
        self.indexed = bool(indexed)
        self.n_rings = self.el_deg.size
        self.n_points = self.n_az * self.n_echo * self.n_rings
        self.n_geometric = 0            # сколько кадров пришлось раскладывать по углам
        self._init_lookup()

        self.az_grid_deg = self.az0_deg + self.az_step_deg * np.arange(self.n_az)

        # карта выборки для выпрямления: out[h, j] = raw[h, j + shift[h]]
        j = np.arange(self.n_az)[None, :]
        src = j + self.col_shift[:, None]
        if self.wrap:
            # у кругового скана «выехавшие» столбцы приходят с другого края
            self.gather = np.mod(src, self.n_az).astype(np.intp)
            self.gather_ok = np.ones_like(self.gather, bool)
        else:
            self.gather_ok = (src >= 0) & (src < self.n_az)
            self.gather = np.clip(src, 0, self.n_az - 1).astype(np.intp)

        self.az_full_deg = self.az_grid_deg[None, :] + self.az_resid_deg[:, None]
        self.dirs = self._unit_dirs().astype(np.float32)        # (H, W, 3)
        # угловой шаг по кольцам: у решётки из одного кольца градиента нет
        self.el_step_deg = (np.abs(np.gradient(self.el_deg)) if self.n_rings > 1
                            else np.full(self.n_rings, 0.125))

    # ------------------------------------------------------------------ геометрия

    def _unit_dirs(self) -> np.ndarray:
        az = self.az_full_deg * DEG
        el = (self.el_deg[:, None] * DEG) * np.ones_like(az)
        c = np.cos(el)
        return np.stack([c * np.sin(az), -c * np.cos(az), np.sin(el)], axis=-1)

    def _init_lookup(self) -> None:
        """Таблицы для проверки порядка точек и поиска кольца по элевации."""
        self._sin_el = np.sin(self.el_deg * DEG).astype(np.float32)
        self._el_order = np.argsort(self.el_deg)
        self._el_asc = self.el_deg[self._el_order]

    def column_slice(self, half_fov_deg: float) -> slice:
        """Непрерывный диапазон столбцов внутри ±half_fov по азимуту."""
        inside = np.flatnonzero(np.abs(self.az_grid_deg) <= half_fov_deg)
        if inside.size == 0:
            return slice(0, self.n_az)
        return slice(int(inside[0]), int(inside[-1]) + 1)

    def sub(self, cols: slice) -> "ScanLayout":
        """Урезанная по азимуту копия решётки (для обработки только переднего сектора)."""
        start = cols.start or 0
        stop = cols.stop if cols.stop is not None else self.n_az
        out = ScanLayout.__new__(ScanLayout)
        out.el_deg = self.el_deg
        out.az_step_deg = self.az_step_deg
        out.az0_deg = float(self.az_grid_deg[start])
        out.col_shift = self.col_shift
        out.az_resid_deg = self.az_resid_deg
        out.n_az = stop - start
        out.n_echo = self.n_echo
        out.wrap = False          # вырезанный сектор больше не смыкается
        out.indexed = self.indexed
        out.n_rings = self.n_rings
        out.n_points = self.n_points
        out.n_geometric = 0
        out._init_lookup()
        out.az_grid_deg = self.az_grid_deg[start:stop]
        out.gather_ok = self.gather_ok[:, start:stop]
        out.gather = self.gather[:, start:stop]
        out.az_full_deg = self.az_full_deg[:, start:stop]
        out.dirs = np.ascontiguousarray(self.dirs[:, start:stop])
        out.el_step_deg = self.el_step_deg
        return out

    # ------------------------------------------------------------------ проекция

    def project(self, pc: PointCloud2, cols: slice | None = None) -> RangeImage:
        """Облако точек → выпрямленный дальностный образ.

        `cols` задаёт нужный диапазон **выходных** столбцов. Тяжёлая
        арифметика (корень по 900 тыс. точек) выполняется только над теми
        сырыми столбцами, которые в этот диапазон попадут с учётом скоса
        каналов, — на круговом скане это экономит почти всё время стадии.

        Кадр с нарушенным порядком точек (см. докстроку модуля) раскладывается
        по углам точек: результат тот же, что дал бы целый кадр.
        """
        w = self.n_az
        start = 0 if cols is None else (cols.start or 0)
        stop = w if cols is None else (cols.stop if cols.stop is not None else w)
        if self.indexed and pc.n_points == self.n_points:
            img = self._project_indexed(pc, start, stop)
            if img is not None:
                return img
        self.n_geometric += 1
        return self._project_geometric(pc, start, stop)

    def _project_indexed(self, pc: PointCloud2, start: int, stop: int) -> RangeImage | None:
        """Быстрый путь по порядку точек. None — порядок в кадре нарушен."""
        n, w, e = self.n_rings, self.n_az, self.n_echo
        lo = start + int(self.col_shift.min())
        hi = stop + int(self.col_shift.max())
        if self.wrap:
            raw_cols = np.arange(lo, hi) % w
        else:
            lo = max(lo, 0)
            hi = min(hi, w)
            raw_cols = None

        pts = pc.points

        def cube(name: str) -> np.ndarray:
            a = pts[name].reshape(w, e, n)
            a = a[raw_cols] if raw_cols is not None else a[lo:hi]
            return a.transpose(2, 0, 1)

        x, y, z = cube("x"), cube("y"), cube("z")
        good = (x != 0) | (y != 0) | (z != 0)
        r = np.sqrt(x * x + y * y + z * z, dtype=np.float32)
        good &= np.isfinite(r)          # драйверы, отдающие «нет эха» как NaN
        r = np.where(good, r, np.float32(0.0))

        # Порядок: элевация каждой точки должна совпасть с элевацией её кольца.
        # У «нет эха» x = y = z = r = 0, и отклонение тоже ноль.
        dev = np.abs(z - r * self._sin_el[:, None, None])
        if np.any(dev > r * np.float32(ORDER_TOL_DEG * DEG) + np.float32(1e-3)):
            return None

        if e == 1:
            r_near = r[..., 0]
            r_far = r[..., 0]
            it = cube("intensity")[..., 0]
            valid = good[..., 0]
        else:
            inten = cube("intensity")
            near_i = np.argmin(np.where(good, r, np.float32(np.inf)), axis=-1)[..., None]
            far_i = np.argmax(r, axis=-1)[..., None]
            r_near = np.take_along_axis(r, near_i, -1)[..., 0]
            r_far = np.take_along_axis(r, far_i, -1)[..., 0]
            it = np.take_along_axis(inten, near_i, -1)[..., 0]
            valid = good.any(axis=-1)

        # выпрямление скоса каналов, с поправкой на смещение окна
        g = self.gather[:, start:stop]
        ok = self.gather_ok[:, start:stop]
        if raw_cols is not None:
            g = (g - lo) % w
        else:
            g = g - lo
            ok = ok & (g >= 0) & (g < (hi - lo))
            np.clip(g, 0, hi - lo - 1, out=g)

        r_near = np.take_along_axis(r_near, g, 1)
        r_far = np.take_along_axis(r_far, g, 1)
        it = np.take_along_axis(it, g, 1)
        valid = np.take_along_axis(valid, g, 1) & ok

        r_near = np.where(valid, r_near, np.float32(0.0))
        r_far = np.where(valid, r_far, np.float32(0.0))
        return RangeImage(pc.stamp, np.ascontiguousarray(r_near),
                          np.ascontiguousarray(r_far), np.ascontiguousarray(it),
                          np.ascontiguousarray(valid))

    def _project_geometric(self, pc: PointCloud2, start: int, stop: int) -> RangeImage:
        """Раскладка по углам: кольцо — по элевации точки, столбец — по азимуту.

        Не опирается на порядок точек вовсе. Если в ячейку попало несколько
        точек (двойное эхо, вставленный предмет поверх фона), ближняя идёт в
        `r_near`, дальняя — в `r_far`, как у целого кадра.
        """
        n, wid = self.n_rings, stop - start
        pts = pc.points
        x = np.asarray(pts["x"], np.float32)
        y = np.asarray(pts["y"], np.float32)
        z = np.asarray(pts["z"], np.float32)
        good = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) \
            & np.isfinite(z)
        idx = np.flatnonzero(good)
        x, y, z = x[idx], y[idx], z[idx]

        # грубый отбор сектора по азимуту, без поправки кольца
        step = self.az_step_deg
        az = np.degrees(np.arctan2(x, -y))
        jf = (az - np.float32(self.az0_deg)) / np.float32(step)
        if self.wrap:
            jf = np.mod(jf, self.n_az)
        margin = float(np.abs(self.az_resid_deg).max()) / abs(step) + 1.0
        sel = np.flatnonzero((jf > start - margin) & (jf < stop - 1 + margin))
        idx, x, y, z, az = idx[sel], x[sel], y[sel], z[sel], az[sel]

        r = np.sqrt(x * x + y * y + z * z)
        el = np.degrees(np.arcsin(np.clip(z / np.maximum(r, np.float32(1e-6)), -1.0, 1.0)))
        # ближайшее кольцо по элевации
        k = np.clip(np.searchsorted(self._el_asc, el), 1, n - 1)
        k -= (el - self._el_asc[k - 1]) < (self._el_asc[k] - el)
        h = self._el_order[k]
        ok = np.abs(el - self.el_deg[h]) <= np.maximum(self.el_step_deg[h], 0.2)
        j = np.rint((az - self.az_resid_deg[h] - self.az0_deg) / step).astype(np.int64)
        if self.wrap:
            j %= self.n_az
        ok &= (j >= start) & (j < stop)
        sel = np.flatnonzero(ok)

        r_near = np.zeros(n * wid, np.float32)
        r_far = np.zeros(n * wid, np.float32)
        it = np.zeros(n * wid, np.float32)
        valid = np.zeros(n * wid, bool)
        if sel.size:
            cell = h[sel] * wid + (j[sel] - start)
            rr = r[sel]
            # сортировка по (ячейка, дальность): первая в ячейке — ближняя
            mm = np.minimum(rr * 1000.0, (1 << 20) - 1).astype(np.int64)
            order = np.argsort(cell * (1 << 20) + mm)
            cs, rs = cell[order], rr[order]
            brk = np.flatnonzero(cs[1:] != cs[:-1])
            first = np.concatenate(([0], brk + 1))
            last = np.concatenate((brk, [cs.size - 1]))
            r_near[cs[first]] = rs[first]
            r_far[cs[last]] = rs[last]
            valid[cs[first]] = True
            if "intensity" in pts.dtype.names:
                src = np.asarray(pts["intensity"], np.float32)[idx[sel]][order]
                it[cs[first]] = src[first]
        return RangeImage(pc.stamp, r_near.reshape(n, wid), r_far.reshape(n, wid),
                          it.reshape(n, wid), valid.reshape(n, wid))

    # ------------------------------------------------------------------ калибровка

    @staticmethod
    def calibrate(clouds: list[PointCloud2], n_rings: int | None = None) -> "ScanLayout":
        """Восстановить решётку по нескольким кадрам.

        Определяются: число колец и эх, элевация каждого кольца, шаг развёртки,
        азимутальный сдвиг каждого канала и целочисленное выпрямление образа.
        """
        if not clouds:
            raise ValueError("нужен хотя бы один кадр для калибровки")
        pc0 = clouds[0]
        if "ring" not in pc0.points.dtype.names:
            # Без поля `ring` (синтетика организаторов): кольца — по гистограмме
            # элевации, а в калибровку по порядку точек идут только кадры, где
            # этот порядок цел. Нет таких — решётка строится целиком по углам.
            el_ring = _ring_elevations(clouds)
            if n_rings is None:
                n_rings = el_ring.size
            whole = [pc for pc in clouds if _is_organized(pc, el_ring)]
            whole = [pc for pc in whole if pc.n_points == whole[0].n_points] if whole else []
            if not whole:
                return ScanLayout._calibrate_geometric(clouds, el_ring)
            clouds, pc0 = whole, whole[0]
        if n_rings is None:
            n_rings = int(pc0.points["ring"].max()) + 1
        n_cols, rem = divmod(pc0.n_points, n_rings)
        if rem:
            raise ValueError(f"{pc0.n_points} точек не делится на {n_rings} колец")

        # Направление луча задано сенсором и в каждом кадре одно и то же:
        # кадры нужны только чтобы закрыть лучи, не вернувшие эхо. Поэтому
        # берётся первое конечное значение, а не медиана по стопке кадров —
        # та стоила 1.4 с из 2.8 с всей калибровки и ничего не уточняла:
        # ниже и шаг развёртки, и сдвиг канала берутся медианой по тысячам
        # столбцов, где шум одного отсчёта всё равно усредняется.
        az = el = None
        for pc in clouds:
            x = pc.points["x"].reshape(n_cols, n_rings).T.astype(np.float32)
            y = pc.points["y"].reshape(n_cols, n_rings).T.astype(np.float32)
            z = pc.points["z"].reshape(n_cols, n_rings).T.astype(np.float32)
            ok = (x != 0) | (y != 0) | (z != 0)
            with _quiet():
                a = np.where(ok, np.degrees(np.arctan2(x, -y)), np.nan)
                e = np.where(ok, np.degrees(np.arctan2(z, np.hypot(x, y))), np.nan)
            if az is None:
                az, el = a, e
                continue
            gap = np.isnan(az)
            if not gap.any():
                break
            az[gap] = a[gap]
            el[gap] = e[gap]
        az = az.astype(np.float64)
        el = el.astype(np.float64)

        n_echo = _detect_echoes(az)
        n_az = n_cols // n_echo
        if n_echo > 1:
            with _quiet():
                az = np.nanmean(az.reshape(n_rings, n_az, n_echo), axis=2)
                el = np.nanmean(el.reshape(n_rings, n_az, n_echo), axis=2)

        # общий шаг развёртки: медиана по кольцам от робастной оценки наклона
        slopes = []
        for h in range(n_rings):
            row = az[h]
            idx = np.flatnonzero(np.isfinite(row))
            if idx.size < 50:
                continue
            d = np.diff(np.unwrap(np.radians(row[idx]))) / np.diff(idx)
            slopes.append(np.median(np.degrees(d)))
        if not slopes:
            raise ValueError("недостаточно валидных лучей для калибровки развёртки")
        step = float(np.median(slopes))

        # смещение каждого канала относительно общей развёртки
        j = np.arange(n_az, dtype=np.float64)
        base = step * j
        with _quiet():
            c_ring = np.nanmedian(_wrap180(az - base[None, :]), axis=1)   # (H,)
        c_ring = _fill_linear(c_ring)
        c0 = float(np.median(c_ring))

        # круговой скан: развёртка покрывает полные 360°
        wrap = abs(step) * n_az > 350.0

        # начало отсчёта выбирается так, чтобы «вперёд» (азимут 0) был в середине
        # образа — иначе шов ±180° разрезал бы рабочий сектор пополам
        if wrap:
            k = int(np.rint(-c0 / step)) - n_az // 2
            c0 = _wrap180(c0 + step * k)
        # разница берётся по кратчайшей дуге: иначе шов ±180° даёт сдвиг в пол-оборота
        shift = np.rint(_wrap180(c0 - c_ring) / step).astype(np.int64)
        resid = _wrap180(c_ring + step * shift - c0)

        with _quiet():
            el_ring = _fill_linear(np.nanmedian(el, axis=1))
        return ScanLayout(el_ring, step, float(c0), shift, resid, n_az, n_echo, wrap)

    @staticmethod
    def _calibrate_geometric(clouds: list[PointCloud2], el_ring: np.ndarray) -> "ScanLayout":
        """Решётка только по углам точек, когда ни в одном кадре нет целого порядка.

        Кольцо — ближайшая элевация из `el_ring`. Шаг развёртки — самая частая
        разность соседних азимутов внутри кольца, сдвиг кольца внутри шага —
        круговое среднее фазы его азимутов. Столбец растёт, азимут убывает — как
        у Pandar128 в наших бэгах, чтобы образ не отразился зеркально.
        """
        n = el_ring.size
        el_desc = np.sort(el_ring)[::-1]
        order = np.argsort(el_desc)
        asc = el_desc[order]
        azs, hs = [], []
        for pc in clouds[:6]:
            x, y, z, ok = _xyz64(pc)
            x, y, z = x[ok], y[ok], z[ok]
            el = np.degrees(np.arctan2(z, np.hypot(x, y)))
            k = np.clip(np.searchsorted(asc, el), 1, n - 1)
            k -= (el - asc[k - 1]) < (asc[k] - el)
            h = order[k]
            on = np.abs(el - el_desc[h]) < ORDER_TOL_DEG
            azs.append(np.degrees(np.arctan2(x[on], -y[on])))
            hs.append(h[on])
        az = np.concatenate(azs)
        h = np.concatenate(hs)

        diffs = []
        for ring in range(n):
            a = np.unique(np.round(az[h == ring], 4))
            if a.size > 20:
                diffs.append(np.diff(a))
        if not diffs:
            raise ValueError("недостаточно валидных лучей для калибровки развёртки")
        d = np.concatenate(diffs)
        d = d[d > 1e-3]
        vals, cnt = np.unique(np.round(d, 3), return_counts=True)
        step = float(vals[np.argmax(cnt)])
        step = float(np.median(d[np.abs(d - step) < 0.1 * step]))

        ph = np.exp(2j * np.pi * az / step)
        s = (np.bincount(h, weights=ph.real, minlength=n)
             + 1j * np.bincount(h, weights=ph.imag, minlength=n))
        ref = float(np.angle(s.sum()) / (2 * np.pi) * step)
        frac = np.where(np.abs(s) > 0, np.angle(s) / (2 * np.pi) * step, ref)
        resid = (frac - ref + step / 2) % step - step / 2

        lo, hi = np.percentile(az, [0.05, 99.95])
        wrap = bool(hi - lo > 350.0)
        if wrap:
            n_az = int(round(360.0 / step))
            az0 = ref + step * round((180.0 - ref) / step)
        else:
            az0 = ref + step * round((hi - ref) / step)
            n_az = int(round((az0 - lo) / step)) + 1
        return ScanLayout(el_desc, -step, float(az0), np.zeros(n, np.int64), resid,
                          n_az, 1, wrap, indexed=False)

    # ------------------------------------------------------------------ сериализация

    def save(self, path: str | Path) -> None:
        np.savez_compressed(path, el_deg=self.el_deg, az_step_deg=self.az_step_deg,
                            az0_deg=self.az0_deg, col_shift=self.col_shift,
                            az_resid_deg=self.az_resid_deg, n_az=self.n_az,
                            n_echo=self.n_echo, wrap=self.wrap, indexed=self.indexed)

    @staticmethod
    def load(path: str | Path) -> "ScanLayout":
        d = np.load(path)
        return ScanLayout(d["el_deg"], float(d["az_step_deg"]), float(d["az0_deg"]),
                          d["col_shift"], d["az_resid_deg"], int(d["n_az"]),
                          int(d["n_echo"]), bool(d["wrap"]) if "wrap" in d else False,
                          bool(d["indexed"]) if "indexed" in d else True)

    def __repr__(self) -> str:
        kind = "" if self.indexed else ", по углам точек"
        return (f"ScanLayout(колец={self.n_rings}, азимутов={self.n_az}, эх={self.n_echo}, "
                f"сектор {self.az_grid_deg.min():.1f}°…{self.az_grid_deg.max():.1f}°, "
                f"шаг {abs(self.az_step_deg):.3f}°, "
                f"элевация {self.el_deg.min():.1f}°…{self.el_deg.max():.1f}°, "
                f"скос каналов {np.ptp(self.col_shift)} стлб{kind})")


# ---------------------------------------------------------------------- вспомогательное

@contextmanager
def _quiet():
    """Пустые срезы и деление на ноль при калибровке — норма, а не ошибка."""
    with warnings.catch_warnings(), np.errstate(all="ignore"):
        warnings.simplefilter("ignore", RuntimeWarning)
        yield


def _wrap180(a):
    """Привести угол(ы) в градусах к полуинтервалу (−180, 180]."""
    return -((-np.asarray(a, np.float64) + 180.0) % 360.0 - 180.0)


def _xyz64(pc: PointCloud2):
    """Координаты в float64 и маска точек с эхом (не ноль и не NaN)."""
    p = pc.points
    x = p["x"].astype(np.float64)
    y = p["y"].astype(np.float64)
    z = p["z"].astype(np.float64)
    ok = ((x != 0) | (y != 0) | (z != 0)) & np.isfinite(x) & np.isfinite(y) & np.isfinite(z)
    return x, y, z, ok


def _ring_elevations(clouds: list[PointCloud2], max_frames: int = 6) -> np.ndarray:
    """Элевации колец по гистограмме, без поля `ring` и без опоры на порядок.

    У лазерного канала элевация постоянна до 0.0001°, поэтому точки кольца
    ложатся в один бин в 0.002°; соседние кольца Pandar128 разнесены на 0.086°
    и больше. Разброс float32 может расщепить кольцо на соседние бины — они
    сливаются. Вставленные точки (синтетика) рассыпаны по элевации и дают
    мелкие группы, которые отсекает порог по весу. Порядок — сверху вниз,
    как нумерует каналы Hesai.
    """
    parts = []
    for pc in clouds[:max_frames]:
        x, y, z, ok = _xyz64(pc)
        parts.append(np.degrees(np.arctan2(z[ok], np.hypot(x[ok], y[ok]))))
    el = np.concatenate(parts)
    if el.size == 0:
        raise ValueError("нет ни одной точки с эхом для калибровки колец")
    q, cnt = np.unique(np.rint(el / 0.002).astype(np.int64), return_counts=True)
    grp = np.concatenate(([0], np.cumsum(np.diff(q) > 5)))
    w = np.bincount(grp, weights=cnt)
    c = np.bincount(grp, weights=cnt * q * 0.002) / w
    top = np.sort(w)[-min(64, w.size):]
    keep = w >= 0.25 * np.median(top)
    if keep.sum() < 2:
        raise ValueError("не удалось выделить кольца по элевации")
    return np.sort(c[keep])[::-1]


def _is_organized(pc: PointCloud2, el_ring: np.ndarray) -> bool:
    """Цел ли порядок точек: элевация каждой точки = элевация её кольца."""
    n = el_ring.size
    if pc.n_points == 0 or pc.n_points % n:
        return False
    x, y, z, ok = _xyz64(pc)
    el = np.degrees(np.arctan2(z, np.hypot(x, y))).reshape(-1, n)
    ok = ok.reshape(-1, n)
    for table in (el_ring, el_ring[::-1]):
        if np.all(np.abs(el - table[None, :])[ok] < ORDER_TOL_DEG):
            return True
    return False


def _detect_echoes(az: np.ndarray) -> int:
    """Двойное эхо: соседние столбцы делят азимут (проверяется по каждому кольцу)."""
    if az.shape[1] < 4:
        return 1
    a, b = az[:, 0::2], az[:, 1::2]
    m = np.isfinite(a) & np.isfinite(b)
    if m.sum() < 100:
        return 1
    return 2 if np.mean(np.abs(a[m] - b[m]) < 1e-3) > 0.8 else 1


def _fill_linear(a: np.ndarray) -> np.ndarray:
    """Линейно достроить NaN-пропуски по индексу (сетка равномерная)."""
    a = np.asarray(a, np.float64).copy()
    bad = ~np.isfinite(a)
    if bad.all():
        raise ValueError("нет ни одного валидного угла для калибровки")
    if bad.any():
        idx = np.arange(a.size)
        a[bad] = np.interp(idx[bad], idx[~bad], a[~bad])
    return a