"""Verify the SLTB reference implementation.

1. Golden vector (spec Appendix A) must encode byte-exact and decode to the listed points.
2. Round trip of docs/ACTIVITY.fit at 5 s with hourly segments (reproduces spec §5).

Run from the repo root:  uvx --with fitdecode python docs/sltb/check.py
Write the ACTIVITY.fit segments as test fixtures:  ... check.py --write-fixtures <dir>
"""
import math, os, sys, gzip
sys.path.insert(0, os.path.dirname(__file__))
from sltb import Encoder, decode, END_FIELDS

GOLDEN = bytes.fromhex(
    "53 56 01 03 6a 63 1b 80 00 00 05 01 00 47 e7 cf"
    "f0 6a 63 1b 80 1d 3a 06 d4 06 af 37 c0 27 94 02"
    "85 0a 79 46 3c e2 00 79 e1 fd 7a e3 02 90 80"
    "f1 00 00 00 00 41 00 00 00 96 00 00 00 50 00 00 00 46"
    "00 00 00 00 00 00 00 1e 00 00 00 23 00 00 00 00 00 00 00 00 02 8a".replace(" ", ""))

GOLDEN_END = dict(zip(END_FIELDS, (0, 65, 150, 80, 70, 0, 30, 35, 0, 0, 650)))


def golden():
    deg2sc = lambda d: math.floor(d * 2**31 / 180 + 0.5)
    lat0, lon0, t0 = 41.1, 9.4, 1784880000
    mlat = 111319.49; mlon = mlat * math.cos(math.radians(lat0))
    path = [(0, 0), (10, 10), (20, 20), (30, 30), (41, 39), (52, 48), (63, 57),
            (74, 66), (85, 75), (96, 84), (107, 93), (140, 93)]
    enc = Encoder(track_id=t0)
    for k, (y, x) in enumerate(path):
        state = 2 if k < 6 else 0                    # sail -> engine at sample 6
        pres = 10132 if k < 8 else 10129             # 1013.2 -> 1012.9 hPa at sample 8
        t = t0 + 5 * k + (10 if k == 11 else 0)      # two samples missing before the last
        enc.point(t, deg2sc(lat0 + y / mlat), deg2sc(lon0 + x / mlon), pres, state)
    enc.close_segment(final=True, end=GOLDEN_END)
    seg = enc.segments[0]
    assert seg == GOLDEN, "golden vector mismatch:\n" + seg.hex(" ")
    d = decode(seg)
    assert [p["t"] - t0 for p in d["points"]] == [0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 65]
    assert d["end"] == GOLDEN_END and d["final"]
    print("golden vector OK (%d bytes)" % len(seg))

    # END-only FINAL segment, as sent when a track is discarded (spec §4.8, §6.2)
    enc = Encoder(track_id=t0); enc.seq = 2
    enc.close_segment(final=True, end={"reason": 1})
    d = decode(enc.segments[0])
    assert d["points"] == [] and d["end"]["reason"] == 1 and d["seq"] == 2 and d["final"]
    print("discard vector OK: " + enc.segments[0].hex(" "))


def load_fit(path):
    import fitdecode
    recs, odo = [], None
    with fitdecode.FitReader(path) as fr:
        for f in fr:
            if not isinstance(f, fitdecode.FitDataMessage):
                continue
            if f.name == "record" and f.has_field("timestamp") and f.get_value("position_lat") is not None:
                speed = f.get_value("enhanced_speed") if f.has_field("enhanced_speed") else None
                recs.append((int(f.get_value("timestamp").timestamp()),
                             f.get_value("position_lat"), f.get_value("position_long"), speed))
            elif f.name == "session" and f.has_field("total_distance"):
                odo = f.get_value("total_distance")
    return recs, odo


def hav(a, b):
    r = 6371008.8
    p1, p2 = math.radians(a["lat"]), math.radians(b["lat"])
    h = math.sin((p2 - p1) / 2) ** 2 + math.cos(p1) * math.cos(p2) * math.sin(math.radians(b["lon"] - a["lon"]) / 2) ** 2
    return 2 * r * math.asin(math.sqrt(h))


def round_trip():
    path = os.path.join(os.path.dirname(__file__), "..", "ACTIVITY.fit")
    recs, odo = load_fit(path)
    t0 = recs[0][0]
    samples = [r for r in recs if (r[0] - t0) % 5 == 0]
    enc, seg_start = Encoder(track_id=t0), t0
    n = 0
    for t, lat, lon, _ in samples:
        if t - seg_start >= 3600:
            enc.close_segment(); seg_start = t
        enc.point(t, lat, lon); n += 1
        if enc.seq == 0 and t - t0 >= 15 and n >= 3:      # first segment: fix + 15 s, >= 3 positions (spec §6.2)
            enc.close_segment(); seg_start = t
    enc.close_segment(final=True, end={"timer_s": samples[-1][0] - t0, "distance_m": int(odo or 0)})

    dec = [p for s in enc.segments for p in decode(s)["points"]]
    assert len(dec) == len(samples)
    err = []
    for d, (t, lat, lon, _) in zip(dec, samples):
        assert d["t"] == t
        la, lo = lat * 180 / 2**31, lon * 180 / 2**31
        err.append(math.hypot((d["lat"] - la) * 111319.49, (d["lon"] - lo) * 111319.49 * math.cos(math.radians(la))))
    total = sum(map(len, enc.segments))
    hours = (samples[-1][0] - t0) / 3600
    dist = sum(hav(dec[i], dec[i + 1]) for i in range(len(dec) - 1))
    sog_d = []
    for i in range(1, len(dec) - 1):
        if samples[i][3] is not None:
            sog = (hav(dec[i - 1], dec[i]) + hav(dec[i], dec[i + 1])) / (dec[i + 1]["t"] - dec[i - 1]["t"])
            sog_d.append(abs(sog - samples[i][3]) * 1.94384)
    sog_d.sort()

    print("ACTIVITY.fit: %.1f h, %d samples, %d segments %s" % (hours, len(samples), len(enc.segments), [len(s) for s in enc.segments]))
    print("  size %d B = %.2f B/sample = %.0f B/h (gzip %d B)" % (total, total / len(samples), total / hours, len(gzip.compress(b"".join(enc.segments), 9))))
    print("  records %s" % enc.stats)
    print("  position error max %.2f m, mean %.2f m" % (max(err), sum(err) / len(err)))
    print("  distance %.2f nm decoded vs %.2f nm odometer" % (dist / 1852, (odo or 0) / 1852))
    print("  SOG |derived - device| median %.2f kn, p95 %.2f kn" % (sog_d[len(sog_d) // 2], sog_d[int(len(sog_d) * .95)]))
    assert max(err) <= 0.71
    if odo:
        assert abs(dist - odo) / odo < 0.005
    return enc.segments


if __name__ == "__main__":
    golden()
    segments = round_trip()
    if "--write-fixtures" in sys.argv:
        out = sys.argv[sys.argv.index("--write-fixtures") + 1]
        os.makedirs(out, exist_ok=True)
        for s in segments:
            seq = int.from_bytes(s[8:10], "big")
            with open(os.path.join(out, "activity_%02d.sltb" % seq), "wb") as f:
                f.write(s)
        print("wrote %d fixtures to %s" % (len(segments), out))
    print("all checks passed")