#!/usr/bin/env python3
"""Kamere za prikaze na stvarnom terenu i rješavanje položaja kamere iz kontrolnih točaka.

Sustav: ENU-M iz scripts/viz/koordinate.py (x istok, y sjever, z = m n. v., pravi metri).
Model: središnja projekcija (pinhole), glavna točka u središtu slike, kvadratni pikseli,
neobvezna radijalna distorzija k1 (samo u rješavanju; render je uvijek bez distorzije).

Kutovi
  azimut_deg  smjer optičke osi, 0 = sjever, 90 = istok (kao GimbalYawDegree)
  nagib_deg   iznad obzora pozitivno, prema tlu negativno (kao GimbalPitchDegree)
  zakret_deg  zakret oko optičke osi; pozitivno = desni rub kamere spušten

Žarišna duljina se čuva u pikselima (f_px) za zadanu širinu slike. Ekvivalent 35 mm:
  "sirina"      f35 = f_px * 36 / sirina_px          (vodoravni kut kao na filmu 36 x 24)
  "dijagonala"  f35 = f_px * 43,267 / dijagonala_px  (fotografska norma; za 3:2 isto)

Modul ne treba Blender; `Kamera.u_blender(scene)` uvozi bpy tek kad se pozove.
Status svih riješenih kamera: PRETPOSTAVKA-IZRAČUN (prilagodba na očitane točke).
"""
from __future__ import annotations

import json
import math
import re
from dataclasses import asdict, dataclass, field
from pathlib import Path

import numpy as np

try:  # rad i kao modul paketa i kao skripta iz scripts/viz
    from . import koordinate as K
except ImportError:  # pragma: no cover
    import koordinate as K

DIJAGONALA_35 = math.hypot(36.0, 24.0)
R_ZEMLJE = 6371000.0
K_REFRAKCIJE = 0.13  # isto kao scripts/viz/vidljivost.py


def pad_zbog_zakrivljenosti(udaljenost_m, k=K_REFRAKCIJE):
    """Prividno spuštanje dalekog cilja ispod ravnine obzora promatrača (m)."""
    return np.asarray(udaljenost_m, dtype=float) ** 2 * (1.0 - k) / (2.0 * R_ZEMLJE)


@dataclass
class Kamera:
    x: float
    y: float
    z: float
    azimut_deg: float
    nagib_deg: float
    f_px: float
    sirina_px: int
    visina_px: int
    zakret_deg: float = 0.0
    k1: float = 0.0
    naziv: str = "kamera"
    izvor: str = ""
    dodatno: dict = field(default_factory=dict)

    # --- geometrija -----------------------------------------------------
    def osi(self):
        """Jedinični vektori kamere u ENU-M: desno, gore, naprijed."""
        a, p, r = (math.radians(v) for v in (self.azimut_deg, self.nagib_deg, self.zakret_deg))
        naprijed = np.array([math.sin(a) * math.cos(p), math.cos(a) * math.cos(p), math.sin(p)])
        desno0 = np.array([math.cos(a), -math.sin(a), 0.0])
        gore0 = np.cross(desno0, naprijed)
        desno = math.cos(r) * desno0 - math.sin(r) * gore0
        gore = math.sin(r) * desno0 + math.cos(r) * gore0
        return desno, gore, naprijed

    def projiciraj(self, tocke):
        """Točke svijeta (N x 3, ENU-M) -> pikseli (N x 2: stupac u, red v), dubina uzduž osi."""
        p = np.atleast_2d(np.asarray(tocke, dtype=float)) - np.array([self.x, self.y, self.z])
        d, g, n = self.osi()
        zc = p @ n
        with np.errstate(divide="ignore", invalid="ignore"):
            xn = (p @ d) / zc
            yn = (p @ g) / zc
        if self.k1:
            fakt = 1.0 + self.k1 * (xn * xn + yn * yn)
            xn, yn = xn * fakt, yn * fakt
        u = self.sirina_px / 2.0 + self.f_px * xn
        v = self.visina_px / 2.0 - self.f_px * yn
        return np.stack([u, v], axis=1), zc

    def zraka(self, u, v):
        """Jedinični smjer zrake kroz piksel (bez distorzije)."""
        d, g, n = self.osi()
        s = n + d * ((u - self.sirina_px / 2.0) / self.f_px) - g * ((v - self.visina_px / 2.0) / self.f_px)
        return s / np.linalg.norm(s)

    # --- žarišna duljina ------------------------------------------------
    def f35(self, nacin="sirina"):
        if nacin == "sirina":
            return self.f_px * 36.0 / self.sirina_px
        return self.f_px * DIJAGONALA_35 / math.hypot(self.sirina_px, self.visina_px)

    def vidni_kut_deg(self):
        return (
            2 * math.degrees(math.atan(self.sirina_px / (2 * self.f_px))),
            2 * math.degrees(math.atan(self.visina_px / (2 * self.f_px))),
        )

    def u_mjerilu(self, sirina_px, visina_px=None):
        """Ista kamera za drugu rezoluciju (isti omjer stranica ako visina nije zadana)."""
        m = sirina_px / self.sirina_px
        v = int(round(self.visina_px * m)) if visina_px is None else int(visina_px)
        k = Kamera(**{**asdict(self), "f_px": self.f_px * m, "sirina_px": int(sirina_px), "visina_px": v})
        return k

    # --- zapis ----------------------------------------------------------
    def u_rjecnik(self):
        lon, lat = K.lonlat_iz_m(self.x, self.y)
        xn, yn = K.n_iz_m(self.x, self.y)
        return {
            "naziv": self.naziv,
            "izvor": self.izvor,
            "koordinate": "ENU-M",
            "x": round(self.x, 3),
            "y": round(self.y, 3),
            "z_mnv": round(self.z, 3),
            "enu_n": [round(float(xn), 2), round(float(yn), 2)],
            "lat": round(float(lat), 7),
            "lon": round(float(lon), 7),
            "azimut_deg": round(self.azimut_deg, 4),
            "nagib_deg": round(self.nagib_deg, 4),
            "zakret_deg": round(self.zakret_deg, 4),
            "f_px": round(self.f_px, 3),
            "sirina_px": self.sirina_px,
            "visina_px": self.visina_px,
            "f35_sirina_mm": round(self.f35("sirina"), 3),
            "f35_dijagonala_mm": round(self.f35("dijagonala"), 3),
            "vidni_kut_deg": [round(v, 3) for v in self.vidni_kut_deg()],
            "k1": self.k1,
            **({"dodatno": self.dodatno} if self.dodatno else {}),
        }

    @staticmethod
    def iz_rjecnika(d):
        return Kamera(
            x=d["x"], y=d["y"], z=d["z_mnv"], azimut_deg=d["azimut_deg"], nagib_deg=d["nagib_deg"],
            zakret_deg=d.get("zakret_deg", 0.0), f_px=d["f_px"], sirina_px=d["sirina_px"],
            visina_px=d["visina_px"], k1=d.get("k1", 0.0), naziv=d.get("naziv", "kamera"),
            izvor=d.get("izvor", ""), dodatno=d.get("dodatno", {}),
        )

    # --- Blender --------------------------------------------------------
    def u_blender(self, scene, objekt=None, blizu=1.0, daleko=60000.0):
        """Stvara ili ažurira Blender kameru i postavlja rezoluciju scene."""
        import bpy
        import mathutils

        if objekt is None:
            objekt = scene.camera
        if objekt is None:
            data = bpy.data.cameras.new("kamera")
            objekt = bpy.data.objects.new("kamera", data)
            scene.collection.objects.link(objekt)
        d, g, n = self.osi()
        m = mathutils.Matrix((
            (d[0], g[0], -n[0], self.x),
            (d[1], g[1], -n[1], self.y),
            (d[2], g[2], -n[2], self.z),
            (0.0, 0.0, 0.0, 1.0),
        ))
        objekt.matrix_world = m
        c = objekt.data
        c.type = "PERSP"
        c.sensor_fit = "HORIZONTAL"
        c.sensor_width = 36.0
        c.lens = self.f_px * 36.0 / self.sirina_px
        c.shift_x = c.shift_y = 0.0
        c.clip_start, c.clip_end = blizu, daleko
        scene.camera = objekt
        scene.render.resolution_x = int(self.sirina_px)
        scene.render.resolution_y = int(self.visina_px)
        scene.render.resolution_percentage = 100
        scene.render.pixel_aspect_x = scene.render.pixel_aspect_y = 1.0
        return objekt


# --- izrada kamera ---------------------------------------------------------

def f_px_iz_f35(f35_mm, sirina_px, visina_px, nacin="sirina"):
    if nacin == "sirina":
        return f35_mm * sirina_px / 36.0
    return f35_mm * math.hypot(sirina_px, visina_px) / DIJAGONALA_35


def iz_polozaja(lat=None, lon=None, *, x=None, y=None, kota_oka=None, visina_iznad_tla=None, tlo=None,
                azimut_deg=0.0, nagib_deg=0.0, zakret_deg=0.0, f35_mm=24.0, ekvivalent="sirina",
                sirina_px=1920, omjer=16 / 9, visina_px=None, naziv="kamera", izvor="zadani položaj"):
    """Kamera iz položaja i smjera.

    Položaj: (lat, lon) u stupnjevima ILI (x, y) u ENU-M. Visina: `kota_oka` (m n. v.) ILI
    `visina_iznad_tla` uz `tlo` (funkcija tlo(x_m, y_m) -> kota, npr. Teren.kota_m iz scena.py).
    """
    if x is None or y is None:
        x, y = K.m_iz_lonlat(lon, lat)
    if kota_oka is None:
        if tlo is None or visina_iznad_tla is None:
            raise ValueError("zadaj kota_oka ili visina_iznad_tla + tlo")
        kota_oka = float(tlo(x, y)) + float(visina_iznad_tla)
    if visina_px is None:
        visina_px = int(round(sirina_px / omjer))
    return Kamera(float(x), float(y), float(kota_oka), float(azimut_deg), float(nagib_deg),
                  f_px_iz_f35(f35_mm, sirina_px, visina_px, ekvivalent), int(sirina_px), int(visina_px),
                  float(zakret_deg), 0.0, naziv, izvor)


def pogled_na(cilj_xyz, *, azimut_od_cilja_deg, udaljenost_m, kota_oka, f35_mm=24.0, sirina_px=1920,
              omjer=16 / 9, ekvivalent="sirina", naziv="kamera"):
    """Kamera koja gleda u točku `cilj_xyz` (ENU-M) s mjesta zadanog smjerom i udaljenošću od cilja."""
    a = math.radians(azimut_od_cilja_deg)
    x = cilj_xyz[0] + udaljenost_m * math.sin(a)
    y = cilj_xyz[1] + udaljenost_m * math.cos(a)
    dx, dy, dz = cilj_xyz[0] - x, cilj_xyz[1] - y, cilj_xyz[2] - kota_oka
    az = math.degrees(math.atan2(dx, dy)) % 360.0
    nag = math.degrees(math.atan2(dz, math.hypot(dx, dy)))
    return iz_polozaja(x=x, y=y, kota_oka=kota_oka, azimut_deg=az, nagib_deg=nag, f35_mm=f35_mm,
                       sirina_px=sirina_px, omjer=omjer, ekvivalent=ekvivalent, naziv=naziv,
                       izvor="pogled na zadanu točku")


def procitaj_dji(put):
    """EXIF i XMP zapis DJI snimke -> rječnik (bez Blendera; treba Pillow)."""
    from PIL import Image

    put = Path(put)
    im = Image.open(put)
    sirina, visina = im.size
    ex = im.getexif()
    gps = ex.get_ifd(0x8825)
    exif = ex.get_ifd(0x8769)

    def stup(v, ref):
        d = float(v[0]) + float(v[1]) / 60.0 + float(v[2]) / 3600.0
        return -d if ref in ("S", "W") else d

    out = {
        "datoteka": put.name,
        "sirina_px": sirina,
        "visina_px": visina,
        "model": str(ex.get(0x0110, "")).strip(),
        "vrijeme": str(exif.get(0x9003, ex.get(0x0132, ""))),
        "f35_exif_mm": float(exif.get(0xA405, 0) or 0) or None,
        "lat": stup(gps[2], gps.get(1, "N")) if 2 in gps else None,
        "lon": stup(gps[4], gps.get(3, "E")) if 4 in gps else None,
        "gps_visina": float(gps[6]) if 6 in gps else None,
    }
    with open(put, "rb") as f:
        glava = f.read(600000)
    i, j = glava.find(b"<x:xmpmeta"), glava.find(b"</x:xmpmeta>")
    xmp = glava[i:j].decode("utf-8", "replace") if i >= 0 and j > i else ""
    for kljuc in ("AbsoluteAltitude", "RelativeAltitude", "GimbalRollDegree", "GimbalYawDegree",
                  "GimbalPitchDegree", "FlightYawDegree", "CalibratedFocalLength", "GpsLatitude", "GpsLongitude"):
        m = re.search(r"drone-dji:%s=\"([^\"]*)\"" % kljuc, xmp)
        if m:
            try:
                out[kljuc] = float(m.group(1))
            except ValueError:
                pass
    return out


def iz_dji(put, tlo=None, naziv=None, smjer="GimbalYawDegree"):
    """Početna kamera iz DJI zapisa. Visina: tlo ispod letjelice + RelativeAltitude ako je
    `tlo` zadan (pretpostavka: polijetanje s razine okolnog tla), inače AbsoluteAltitude.
    Apsolutna visina u DJI zapisu nije pouzdana; ovu kameru treba riješiti na kontrolne točke.
    `smjer`: polje XMP zapisa iz kojeg se uzima azimut. Na snimci DJI_2025_0004 GimbalYawDegree
    (-139,2) ne odgovara snimci, a FlightYawDegree (113,4) odgovara do 0,1 stupnja; pri sumnji
    usporedi oba s položajem poznatih objekata na snimci.
    """
    d = procitaj_dji(put)
    lat = d.get("GpsLatitude", d["lat"])
    lon = d.get("GpsLongitude", d["lon"])
    x, y = K.m_iz_lonlat(lon, lat)
    if tlo is not None and "RelativeAltitude" in d:
        z = float(tlo(x, y)) + d["RelativeAltitude"]
        z_izvor = "tlo ispod letjelice + RelativeAltitude"
    else:
        z = d.get("AbsoluteAltitude", d.get("gps_visina") or 0.0)
        z_izvor = "AbsoluteAltitude (nepouzdano)"
    w, h = d["sirina_px"], d["visina_px"]
    if d.get("CalibratedFocalLength"):
        f_px = d["CalibratedFocalLength"]
        f_izvor = "CalibratedFocalLength (px)"
    else:
        # ekvivalent 35 mm odnosi se na puni senzor; DJI 16:9 je izrez senzora 4:3 pune širine
        puna_visina = max(h, int(round(w * 3 / 4))) if w / h > 1.4 else h
        f_px = f_px_iz_f35(d.get("f35_exif_mm") or 24.0, w, puna_visina, "dijagonala")
        f_izvor = "FocalLengthIn35mmFilm, dijagonala punog senzora (PRETPOSTAVKA)"
    return Kamera(float(x), float(y), float(z), d.get(smjer, d.get("GimbalYawDegree", 0.0)) % 360.0,
                  d.get("GimbalPitchDegree", 0.0), float(f_px), w, h, d.get("GimbalRollDegree", 0.0), 0.0,
                  naziv or Path(put).stem, "EXIF/XMP DJI",
                  {"exif": d, "z_izvor": z_izvor, "f_izvor": f_izvor, "azimut_izvor": smjer})


# --- rješavanje kamere -------------------------------------------------------

PARAMETRI = ("x", "y", "z", "azimut_deg", "nagib_deg", "zakret_deg", "f_px", "k1")


def _udaljenost_od_pravca(kam, a, b, pikseli):
    """Predznačena udaljenost (px) očitanih piksela od slike pravca kroz točke svijeta a i b."""
    (pa, pb), zc = kam.projiciraj(np.array([a, b], dtype=float))
    if np.any(zc <= 0):
        return np.full(len(pikseli), 1e4)
    s = pb - pa
    n = np.array([-s[1], s[0]]) / max(float(np.hypot(*s)), 1e-9)
    return (np.asarray(pikseli, dtype=float) - pa) @ n


def rijesi_kameru(pocetna, svijet, pikseli, slobodno=("z", "azimut_deg", "nagib_deg", "zakret_deg", "f_px"),
                  prior=None, sigma_px=3.0, tezine=None, robustno=False, pravci=None):
    """Najmanji kvadrati pogreške reprojekcije.

    pocetna   Kamera (npr. iz_dji)
    svijet    N x 3 kontrolne točke u ENU-M
    pikseli   N x 2 očitani pikseli (u stupac, v red; ishodište gornji lijevi kut)
    slobodno  parametri koji se rješavaju (podskup PARAMETRI)
    prior     {parametar: sigma} meka veza na početnu vrijednost (npr. {"x": 5, "y": 5} za GPS
              ili {"f_px": 0.03 * f} za žarišnu duljinu); jedinice parametra
    sigma_px  pretpostavljena točnost očitanja piksela (određuje težinu priora)
    tezine    neobvezne težine po točki (npr. manja za nesigurnu daleku točku)
    pravci    neobvezno: popis {"a": [x, y, z], "b": [x, y, z], "pikseli": [[u, v], ...], "tezina": 1.0}.
              Pravac u svijetu (ENU-M) zadan dvjema točkama i pikseli očitani bilo gdje uzduž njegove slike
              (rub ceste, crta, podnožje ograde). Ostatak je udaljenost piksela od slike pravca, pa pravac
              veže kameru samo okomito na sebe; služi gdje nema pouzdanih točaka (prednji plan).
    Vraća (Kamera, izvještaj). Izvještaj: rms_px, max_px, ostaci po točki, ostaci po pravcu, kovarijanca.
    """
    from scipy.optimize import least_squares

    svijet = np.asarray(svijet, dtype=float).reshape(-1, 3)
    pikseli = np.asarray(pikseli, dtype=float).reshape(-1, 2)
    n = len(svijet)
    tezine = np.ones(n) if tezine is None else np.asarray(tezine, dtype=float)
    prior = dict(prior or {})
    pravci = list(pravci or [])
    slobodno = tuple(slobodno)
    for p in slobodno:
        if p not in PARAMETRI:
            raise ValueError(f"nepoznat parametar {p}")
    p0 = np.array([getattr(pocetna, p) for p in slobodno], dtype=float)
    # mjerila da svi parametri budu reda veličine 1
    mjerilo = np.array([{"x": 10.0, "y": 10.0, "z": 10.0, "azimut_deg": 1.0, "nagib_deg": 1.0, "zakret_deg": 1.0,
                         "f_px": pocetna.f_px * 0.01, "k1": 0.01}[p] for p in slobodno])

    def kamera_od(q):
        d = asdict(pocetna)
        for p, v in zip(slobodno, p0 + q * mjerilo):
            d[p] = float(v)
        return Kamera(**d)

    def ostaci(q):
        kam = kamera_od(q)
        uv, zc = kam.projiciraj(svijet)
        r = (uv - pikseli) * tezine[:, None]
        r[zc <= 0] = 1e4  # točka iza kamere
        dodatak = [(getattr(kam, p) - getattr(pocetna, p)) / s * sigma_px for p, s in prior.items() if p in slobodno]
        crte = [_udaljenost_od_pravca(kam, pr["a"], pr["b"], pr["pikseli"]) * float(pr.get("tezina", 1.0)) for pr in pravci]
        return np.concatenate([r.ravel(), np.array(dodatak, dtype=float)] + crte)

    rez = least_squares(ostaci, np.zeros(len(slobodno)), loss="soft_l1" if robustno else "linear",
                        f_scale=3 * sigma_px, x_scale=1.0, max_nfev=2000)
    kam = kamera_od(rez.x)
    kam.izvor = "riješeno najmanjim kvadratima reprojekcije na kontrolne točke"
    kam.azimut_deg %= 360.0
    uv, zc = kam.projiciraj(svijet)
    d = uv - pikseli
    e = np.hypot(d[:, 0], d[:, 1])
    stupnjevi = (2 * n + sum(1 for p in prior if p in slobodno) + sum(len(pr["pikseli"]) for pr in pravci)
                 - len(slobodno))
    ost_pravaca = []
    for pr in pravci:
        o = _udaljenost_od_pravca(kam, pr["a"], pr["b"], pr["pikseli"])
        ost_pravaca.append({"naziv": pr.get("naziv", ""), "ostaci_px": [round(float(v), 2) for v in o],
                            "rms_px": float(np.sqrt(np.mean(o ** 2))), "max_px": float(np.abs(o).max())})
    kov = None
    try:
        jtj = rez.jac.T @ rez.jac
        s2 = float(np.sum(rez.fun ** 2) / max(stupnjevi, 1))
        kov = np.linalg.inv(jtj) * s2
        sig = np.sqrt(np.diag(kov)) * mjerilo
    except np.linalg.LinAlgError:
        sig = np.full(len(slobodno), np.nan)
    izvjestaj = {
        "broj_tocaka": n,
        "slobodni_parametri": list(slobodno),
        "prior_sigma": prior,
        "sigma_px": sigma_px,
        "rms_px": float(np.sqrt(np.mean(e ** 2))),
        "max_px": float(e.max()),
        "rms_posto_sirine": float(np.sqrt(np.mean(e ** 2)) / kam.sirina_px * 100.0),
        "max_posto_sirine": float(e.max() / kam.sirina_px * 100.0),
        "ostaci_px": [[round(float(a), 2), round(float(b), 2)] for a, b in d],
        "pogreska_px": [round(float(v), 2) for v in e],
        "pravci": ost_pravaca,
        "procjena_sigma_parametara": {p: (None if not np.isfinite(s) else round(float(s), 4)) for p, s in zip(slobodno, sig)},
        "pomak_od_pocetne": {p: round(float(getattr(kam, p) - getattr(pocetna, p)), 4) for p in slobodno},
        "uspjeh": bool(rez.success),
        "stupnjevi_slobode": int(stupnjevi),
    }
    return kam, izvjestaj


def izostavi_jednu(pocetna, svijet, pikseli, **kw):
    """Unakrsna provjera: svaka točka se izostavi, kamera riješi na ostalima i izmjeri
    pogreška na izostavljenoj. Vraća popis pogrešaka (px) i njihov RMS."""
    svijet = np.asarray(svijet, dtype=float)
    pikseli = np.asarray(pikseli, dtype=float)
    tez = kw.pop("tezine", None)
    out = []
    for i in range(len(svijet)):
        m = np.arange(len(svijet)) != i
        kam, _ = rijesi_kameru(pocetna, svijet[m], pikseli[m], tezine=None if tez is None else np.asarray(tez)[m], **kw)
        uv, _ = kam.projiciraj(svijet[i])
        out.append(float(np.hypot(*(uv[0] - pikseli[i]))))
    return out, float(np.sqrt(np.mean(np.square(out))))


def _samoprovjera():
    """Sintetička provjera: poznata kamera -> pikseli -> rješenje iz pomaknute početne."""
    rng = np.random.default_rng(7)
    prava = Kamera(120.0, -640.0, 205.0, 11.0, -4.0, 3650.0, 5472, 3648, 0.6)
    tocke = np.column_stack([rng.uniform(-500, 500, 14), rng.uniform(-300, 900, 14), rng.uniform(112, 150, 14)])
    uv, zc = prava.projiciraj(tocke)
    assert (zc > 0).all()
    sum_px = uv + rng.normal(0, 1.0, uv.shape)
    poc = Kamera(123.0, -636.0, 190.0, 14.0, -2.0, 3700.0, 5472, 3648, 0.0)
    kam, izv = rijesi_kameru(poc, tocke, sum_px, slobodno=("x", "y", "z", "azimut_deg", "nagib_deg", "zakret_deg", "f_px"))
    assert izv["rms_px"] < 2.5, izv["rms_px"]
    assert abs(kam.z - prava.z) < 3.0 and abs(kam.azimut_deg - prava.azimut_deg) < 0.2, kam
    # osi su ortonormirane i desne
    d, g, n = prava.osi()
    assert abs(np.dot(d, g)) < 1e-12 and abs(np.dot(d, n)) < 1e-12
    assert np.allclose(np.cross(d, g), -n)  # desno x gore = natrag (kamera gleda niz -Z kao u Blenderu)
    # kamera prema sjeveru: točka istočno je desno, točka više je gore
    k = Kamera(0, 0, 100, 0, 0, 1000, 2000, 1000)
    uv, _ = k.projiciraj([[10, 100, 100], [0, 100, 110]])
    assert uv[0, 0] > 1000 and abs(uv[0, 1] - 500) < 1e-9 and uv[1, 1] < 500
    # pozitivan zakret: desni rub spušten -> obzor se na slici diže udesno (manji v desno)
    k2 = Kamera(0, 0, 100, 0, 0, 1000, 2000, 1000, 5.0)
    uv2, _ = k2.projiciraj([[-50, 100, 100], [50, 100, 100]])
    assert uv2[1, 1] < uv2[0, 1]
    # pravac: pikseli očitani uzduž slike pravca imaju ostatak 0; pomak piksela okomito daje taj pomak
    a, b = np.array([-100.0, 0.0, 118.0]), np.array([100.0, 60.0, 119.0])
    na = np.array([a + t * (b - a) for t in (0.1, 0.5, 0.9)])
    uvp, _ = prava.projiciraj(na)
    assert np.abs(_udaljenost_od_pravca(prava, a, b, uvp)).max() < 1e-6
    s_ = uvp[2] - uvp[0]
    okomica = np.array([-s_[1], s_[0]]) / np.hypot(*s_)
    assert np.allclose(np.abs(_udaljenost_od_pravca(prava, a, b, uvp + 5.0 * okomica)), 5.0, atol=1e-6)
    return izv["rms_px"]


if __name__ == "__main__":
    import argparse
    import sys

    ap = argparse.ArgumentParser(description="Kamere: samoprovjera, čitanje DJI zapisa, ponovno rješavanje validacije")
    ap.add_argument("naredba", nargs="?", default="provjera", choices=["provjera", "dji", "validacija"])
    ap.add_argument("put", nargs="?")
    a = ap.parse_args()
    if a.naredba == "provjera":
        print(f"sintetička provjera prošla, RMS {_samoprovjera():.2f} px (šum 1 px)")
    elif a.naredba == "dji":
        print(json.dumps(iz_dji(a.put).u_rjecnik(), ensure_ascii=False, indent=1))
    else:
        put = Path(a.put) if a.put else Path(__file__).resolve().parents[2] / "research/viz/model/validacija_kamere.json"
        v = json.loads(put.read_text(encoding="utf-8"))
        kod = 0
        for s in v["snimke"]:
            poc = Kamera.iz_rjecnika(s["pocetna_kamera"])
            t = [p for p in s["kontrolne_tocke"] if p.get("koristi", True)]
            sv = np.array([p["enu_m"] for p in t])
            px = np.array([p["piksel"] for p in t])
            pravci = [{"a": q["a_enu_m"], "b": q["b_enu_m"], "pikseli": q["pikseli"], "tezina": q.get("tezina", 1.0),
                       "naziv": q["naziv"]} for q in s.get("kontrolni_pravci", []) if q.get("koristi", True)]
            kam, izv = rijesi_kameru(poc, sv, px, slobodno=s["rjesavanje"]["slobodno"], prior=s["rjesavanje"].get("prior"),
                                     sigma_px=s["rjesavanje"].get("sigma_px", 3.0),
                                     tezine=[p.get("tezina", 1.0) for p in t], pravci=pravci)
            zap = s.get("pogreske", {}).get("rms_px")
            if zap is not None and abs(zap - izv["rms_px"]) > 0.05:
                print(f"UPOZORENJE {s['naziv']}: zapisani RMS {zap} px, ponovno izračunat {izv['rms_px']:.2f} px")
                kod = 1
            print(f"{s['naziv']}: {len(t)} točaka, RMS {izv['rms_px']:.1f} px ({izv['rms_posto_sirine']:.3f} % širine), "
                  f"najveća {izv['max_px']:.1f} px; z {kam.z:.1f}, azimut {kam.azimut_deg:.2f}, nagib {kam.nagib_deg:.2f}, "
                  f"zakret {kam.zakret_deg:.2f}, f35 {kam.f35():.2f} mm")
            for q in izv["pravci"]:
                print(f"   pravac {q['naziv']}: RMS {q['rms_px']:.1f} px, najveća {q['max_px']:.1f} px")
        sys.exit(kod)
