"""LRPT telemeetria (APID 70) ja ajatemplid tekstiks. Kasutus: python lrpt_telemetry.py captures/lrpt/<nimi>
Väljund: 9_telemeetria.txt – iga APID-70 paketi aeg + toorbaidid, ning väljad, mis ülelennu jooksul muutuvad/ei muutu."""
import os, sys
from datetime import datetime, timedelta, timezone
import numpy as np

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from lrpt_image import packets_from_vcdus, pkt_time


def main():
    folder = sys.argv[1]
    v = np.load(os.path.join(folder, "6_vcdu.npy")); m = np.load(os.path.join(folder, "6_vcdu_meta.npy"))
    sel = sorted([(int(m[i][2]), bool(m[i][3]), v[i]) for i in range(len(v)) if m[i][1] == 5], key=lambda x: x[0])
    pk = packets_from_vcdus(sel)
    tm = [p for p in pk if p[0] == 70 and len(p[2]) >= 20]
    img = [p for p in pk if 64 <= p[0] <= 69 and len(p[2]) >= 14]
    lines = []
    lines.append(f"Pakette kokku {len(pk)}; telemeetria (APID 70) {len(tm)}; pildipakette {len(img)}")
    if img:
        ts = sorted(pkt_time(d) for a, s, d in img)
        med = np.median(ts); ts = [t for t in ts if abs(t - med) < 420]
        def fmt(t):
            day = int(t // 86400); sod = t - day * 86400
            return f"päev {day} (= {datetime(2000,1,1)+timedelta(days=day):%Y-%m-%d} satelliidi epohhis) {int(sod//3600):02d}:{int(sod%3600//60):02d}:{sod%60:06.3f} UTC"
        lines.append(f"Pildiandmete esimene ajatempel: {fmt(ts[0])}")
        lines.append(f"Pildiandmete viimane ajatempel: {fmt(ts[-1])}  (kestus {ts[-1]-ts[0]:.1f} s)")
        qfs = [d[13] for a, s, d in img]
        lines.append(f"Kvaliteeditegur (QF) pildipakettides: min {min(qfs)}, max {max(qfs)}, mediaan {int(np.median(qfs))}")
        lens = [len(d) for a, s, d in img]
        lines.append(f"Pildipaketi pikkus: min {min(lens)}, max {max(lens)}, keskmine {np.mean(lens):.0f} baiti (14 plokki)")
    lines.append("")
    lines.append("APID 70 – telemeetria (toorbaidid pärast 8-baidist ajatemplit; formaat pole avalikult dokumenteeritud):")
    rows = []
    for a, s, d in tm:
        t = pkt_time(d); sod = t % 86400
        payload = bytes(d[8:])
        rows.append((sod, s, payload))
        lines.append(f"  {int(sod//3600):02d}:{int(sod%3600//60):02d}:{sod%60:06.3f} seq {s:5d} len {len(d):3d}: " + payload.hex(" "))
    if len(rows) >= 2:
        L = min(len(r[2]) for r in rows)
        arr = np.array([list(r[2][:L]) for r in rows], np.uint8)
        const = [i for i in range(L) if (arr[:, i] == arr[0, i]).all()]
        var = [i for i in range(L) if i not in const]
        lines.append("")
        lines.append(f"Muutumatud baidid ({len(const)}/{L}): positsioonid {const}")
        lines.append(f"Muutuvad baidid ({len(var)}): positsioonid {var}")
        for i in var[:20]:
            vals = arr[:, i]
            lines.append(f"  bait {i}: min {vals.min()}, max {vals.max()}, unikaalseid {len(set(vals.tolist()))}, jada algus {vals[:12].tolist()}")
    # Leitud struktuur (19.09.2026, M2-4): payload[66:82] = 4× float32 = orientatsiooni kvaternioon (|q| = 1,000),
    # payload[86:110] = 3× float64 = satelliidi asukoht ECEF meetrites (r ≈ 7194 km = Maa raadius + ~820 km).
    import struct, math
    try:
        from lrpt_geo import sat_state, ecef_to_ll
        from passes import load_tles
        from sgp4.api import Satrec
        sats = load_tles(); key = [k for k in sats if k.startswith("METEOR-M2 4")]
        sat = Satrec.twoline2rv(*sats[key[0]]) if key else None
    except Exception:
        sat = None
    lines.append("")
    lines.append("Dekodeeritud väljad: aeg (UTC) | kvaternioon q0..q3 | |q| | asukoht ECEF x,y,z (km) | r (km) | alampunkt | sgp4 alampunkt (TLE)")
    csv = ["t_utc,q0,q1,q2,q3,x_km,y_km,z_km,r_km,lat,lon,sgp4_lat,sgp4_lon"]
    for sod, s, p in rows:
        if len(p) < 110:
            continue
        q = struct.unpack(">4f", p[66:82]); pos = struct.unpack(">3d", p[86:110])
        nq = math.sqrt(sum(x * x for x in q)); r = math.sqrt(sum(x * x for x in pos))
        if not (0.98 < nq < 1.02) or not (6.9e6 < r < 7.5e6):
            continue
        la = math.degrees(math.atan2(pos[2], math.hypot(pos[0], pos[1]))); lo = math.degrees(math.atan2(pos[1], pos[0]))
        s_la = s_lo = float("nan")
        if sat is not None:
            tt = datetime.now(timezone.utc).replace(hour=0, minute=0, second=0, microsecond=0) + timedelta(seconds=sod)
            try:
                r_e, _ = sat_state(sat, tt); s_la, s_lo = ecef_to_ll(r_e)
            except Exception:
                pass
        ts = f"{int(sod//3600):02d}:{int(sod%3600//60):02d}:{sod%60:04.1f}"
        csv.append(f"{ts},{q[0]:.4f},{q[1]:.4f},{q[2]:.4f},{q[3]:.4f},{pos[0]/1e3:.1f},{pos[1]/1e3:.1f},{pos[2]/1e3:.1f},{r/1e3:.1f},{la:.3f},{lo:.3f},{s_la:.3f},{s_lo:.3f}")
        if len(csv) % 25 == 2:
            lines.append(f"  {ts} | ({q[0]:+.3f},{q[1]:+.3f},{q[2]:+.3f},{q[3]:+.3f}) | {nq:.3f} | ({pos[0]/1e3:.0f},{pos[1]/1e3:.0f},{pos[2]/1e3:.0f}) | {r/1e3:.0f} | {la:.2f}N {lo:.2f}E | {s_la:.2f}N {s_lo:.2f}E")
    open(os.path.join(folder, "9_telemeetria_asend.csv"), "w", encoding="utf-8").write("\n".join(csv))
    lines.append(f"  (kokku {len(csv)-1} kehtivat asukoha/orientatsiooni kirjet -> 9_telemeetria_asend.csv)")
    out = os.path.join(folder, "9_telemeetria.txt")
    open(out, "w", encoding="utf-8").write("\n".join(lines))
    print("\n".join(lines[:12])); print(f"... -> {out}")


if __name__ == "__main__":
    main()
