#!/usr/bin/env python3
"""NTRIP 캐스터 조사/검증 도구 — 마운트포인트가 '진짜 단일기준국'인지 판별한다.

VRS(가상 기지국)와 단일기준국(실제 상시관측소)은 같은 RTCM3 스트림으로 오지만
메시지 구성이 다르다. 이 도구는 스트림을 직접 뜯어서 구분한다.

  table  캐스터 소스테이블 출력 (인증 불필요)
  near   내 좌표에서 가까운 기준국 마운트포인트 정렬
  sniff  마운트포인트에 접속해 RTCM 메시지 종류/기지국 좌표/베이스라인을 관찰
         → 기지국 좌표가 시간에 따라 움직이면 VRS, 고정이면 실제 기지국

사용 예:
  python3 ntrip_probe.py table gnssdata.or.kr 2101
  python3 ntrip_probe.py near  gnssdata.or.kr 2101 37.5665 126.9780
  python3 ntrip_probe.py sniff RTS1.ngii.go.kr 2101 VRS-RTCM34 \
      --user ID --pass PW --gga-from-latlon 37.5665 126.9780 --secs 60
"""

import argparse
import base64
import math
import socket
import sys
import time
from collections import Counter

# ── RTCM3 프레이밍 ──────────────────────────────────────────────────────────
# 0xD3 | 6bit reserved + 10bit length | payload | 3byte CRC-24Q

_CRC24_TAB = []


def _init_crc24():
    poly = 0x1864CFB
    for i in range(256):
        crc = i << 16
        for _ in range(8):
            crc <<= 1
            if crc & 0x1000000:
                crc ^= poly
        _CRC24_TAB.append(crc & 0xFFFFFF)


_init_crc24()


def crc24q(data: bytes) -> int:
    crc = 0
    for b in data:
        crc = ((crc << 8) & 0xFFFFFF) ^ _CRC24_TAB[((crc >> 16) ^ b) & 0xFF]
    return crc


class Bits:
    """MSB-first 비트 리더 (RTCM DF 필드용)."""

    def __init__(self, buf: bytes):
        self.buf = buf
        self.pos = 0

    def u(self, n: int) -> int:
        v = 0
        for _ in range(n):
            byte = self.buf[self.pos >> 3]
            v = (v << 1) | ((byte >> (7 - (self.pos & 7))) & 1)
            self.pos += 1
        return v

    def s(self, n: int) -> int:
        v = self.u(n)
        return v - (1 << n) if v & (1 << (n - 1)) else v


def parse_1005_1006(payload: bytes):
    """1005/1006 → (station_id, X, Y, Z, antenna_height|None). 단위 m."""
    b = Bits(payload)
    msg = b.u(12)
    if msg not in (1005, 1006):
        return None
    sid = b.u(12)
    b.u(6)                      # ITRF realization year
    b.u(1); b.u(1); b.u(1)      # GPS / GLONASS / Galileo indicator
    b.u(1)                      # reference station indicator
    x = b.s(38) * 1e-4
    b.u(1); b.u(1)              # single receiver osc / reserved
    y = b.s(38) * 1e-4
    b.u(2)                      # quarter cycle indicator
    z = b.s(38) * 1e-4
    h = b.u(16) * 1e-4 if msg == 1006 else None
    return sid, x, y, z, h


def ecef_to_lla(x, y, z):
    a, f = 6378137.0, 1 / 298.257223563
    e2 = f * (2 - f)
    lon = math.atan2(y, x)
    p = math.hypot(x, y)
    lat = math.atan2(z, p * (1 - e2))
    for _ in range(10):
        n = a / math.sqrt(1 - e2 * math.sin(lat) ** 2)
        alt = p / math.cos(lat) - n
        lat = math.atan2(z, p * (1 - e2 * n / (n + alt)))
    n = a / math.sqrt(1 - e2 * math.sin(lat) ** 2)
    return math.degrees(lat), math.degrees(lon), p / math.cos(lat) - n


def haversine_km(lat1, lon1, lat2, lon2):
    r = 6371.0
    dlat = math.radians(lat2 - lat1)
    dlon = math.radians(lon2 - lon1)
    h = (math.sin(dlat / 2) ** 2
         + math.cos(math.radians(lat1)) * math.cos(math.radians(lat2)) * math.sin(dlon / 2) ** 2)
    return 2 * r * math.asin(math.sqrt(h))


# 네트워크 RTK(=VRS/FKP/MAC) 임을 드러내는 메시지들
NETWORK_MSGS = {
    1014: "Network Auxiliary Station Data (MAC)",
    1015: "GPS Ionospheric Correction Differences (MAC)",
    1016: "GPS Geometric Correction Differences (MAC)",
    1017: "GPS Combined Correction Differences (MAC)",
    1030: "GPS Network RTK Residual",
    1031: "GLONASS Network RTK Residual",
    1032: "Physical Reference Station Position (VRS)",
    1034: "GPS Network FKP Gradient",
    1035: "GLONASS Network FKP Gradient",
}


# ── NTRIP ───────────────────────────────────────────────────────────────────
def ntrip_open(host, port, mount=None, user="", pw="", timeout=15):
    s = socket.create_connection((host, port), timeout=timeout)
    path = "/" + (mount or "")
    req = f"GET {path} HTTP/1.0\r\nUser-Agent: NTRIP ntrip_probe/1.0\r\nAccept: */*\r\n"
    if user or pw:
        auth = base64.b64encode(f"{user}:{pw}".encode()).decode()
        req += f"Authorization: Basic {auth}\r\n"
    req += "Connection: close\r\n\r\n"
    s.sendall(req.encode())
    return s


def read_header(s):
    """응답 헤더를 소진하고 (상태줄, 남은 바이트) 반환."""
    buf = b""
    while b"\r\n\r\n" not in buf and b"\n\n" not in buf:
        chunk = s.recv(4096)
        if not chunk:
            break
        buf += chunk
        if buf.startswith(b"ICY 200 OK") and len(buf) > 12:  # NTRIP1 짧은 응답
            break
        if len(buf) > 65536:
            break
    sep = b"\r\n\r\n" if b"\r\n\r\n" in buf else b"\n\n"
    head, _, rest = buf.partition(sep)
    status = head.split(b"\r\n")[0].decode("latin1", "replace")
    return status, head, rest


def fetch_sourcetable(host, port):
    s = ntrip_open(host, port)
    data = b""
    try:
        while True:
            chunk = s.recv(8192)
            if not chunk:
                break
            data += chunk
    finally:
        s.close()
    return data.decode("latin1", "replace")


def parse_str_rows(text, host=""):
    # 캐스터가 짧은 간격의 재요청을 조용히 끊는 일이 있다 — 빈 결과를 정상으로 오인하면 안 된다.
    if "STR;" not in text:
        print(f"# ⚠️ 소스테이블 응답에 STR 항목이 없음 ({len(text)} bytes)"
              + (f" — {host} 재시도해볼 것" if host else ""), file=sys.stderr)
        if text[:200].strip():
            print("# 응답 앞부분: " + repr(text[:200]), file=sys.stderr)
    rows = []
    for line in text.splitlines():
        if not line.startswith("STR;"):
            continue
        f = line.split(";")
        if len(f) < 13:
            continue
        try:
            lat, lon = float(f[9]), float(f[10])
        except ValueError:
            lat = lon = None
        rows.append({
            "mount": f[1], "format": f[3], "details": f[4], "nav": f[6],
            "network": f[7], "lat": lat, "lon": lon,
            "nmea": f[11], "solution": f[12],
            "generator": f[13] if len(f) > 13 else "",
            "auth": f[15] if len(f) > 15 else "",
        })
    return rows


def nmea_gga(lat, lon, alt=50.0):
    def dm(v, deg_w):
        d = int(abs(v))
        m = (abs(v) - d) * 60
        return f"{d:0{deg_w}d}{m:09.6f}"
    t = time.gmtime()
    body = (f"GPGGA,{t.tm_hour:02d}{t.tm_min:02d}{t.tm_sec:02d}.00,"
            f"{dm(lat,2)},{'N' if lat>=0 else 'S'},"
            f"{dm(lon,3)},{'E' if lon>=0 else 'W'},"
            f"1,10,1.0,{alt:.1f},M,20.0,M,,")
    ck = 0
    for c in body:
        ck ^= ord(c)
    return f"${body}*{ck:02X}\r\n".encode()


# ── 서브커맨드 ──────────────────────────────────────────────────────────────
def cmd_table(a):
    text = fetch_sourcetable(a.host, a.port)
    rows = parse_str_rows(text, a.host)
    print(f"# {a.host}:{a.port} — STR {len(rows)}개")
    print(f"{'MOUNTPOINT':<18} {'FORMAT':<9} {'NAV':<34} {'NET':<12} {'NMEA':<5} {'SOL':<4} {'종류'}")
    for r in rows:
        if a.filter and a.filter.upper() not in r["mount"].upper():
            continue
        kind = "네트워크(VRS/FKP/MAC)" if r["solution"] == "1" else "단일기준국"
        print(f"{r['mount']:<18} {r['format']:<9} {r['nav'][:34]:<34} "
              f"{r['network'][:12]:<12} {r['nmea']:<5} {r['solution']:<4} {kind}")
    net = sum(1 for r in rows if r["solution"] == "1")
    print(f"\n요약: 단일기준국 {len(rows)-net}개 / 네트워크 {net}개")
    print("  · SOL=0 → 실제 기준국 1곳의 관측치(단일기준국)")
    print("  · SOL=1 → 망 보간 해(VRS/FKP/MAC). NMEA=1 이면 GGA 업링크 필요")


def cmd_near(a):
    rows = parse_str_rows(fetch_sourcetable(a.host, a.port), a.host)
    cand = []
    for r in rows:
        if r["lat"] is None or (r["lat"] == 0 and r["lon"] == 0):
            continue
        if a.format and a.format.upper() not in r["format"].upper().replace(" ", ""):
            continue
        cand.append((haversine_km(a.lat, a.lon, r["lat"], r["lon"]), r))
    cand.sort(key=lambda t: t[0])
    print(f"# 기준 위치 {a.lat:.5f}, {a.lon:.5f} — 가까운 순 {a.n}개"
          + (f" (포맷 필터: {a.format})" if a.format else ""))
    print(f"{'거리km':>8}  {'MOUNTPOINT':<18} {'FORMAT':<9} {'NAV':<34} {'SOL'}")
    for d, r in cand[:a.n]:
        print(f"{d:8.1f}  {r['mount']:<18} {r['format']:<9} {r['nav'][:34]:<34} {r['solution']}")
    print("\n주의: 소스테이블 좌표는 반올림/오기입이 흔하다(0.1도 = 약 11km).")
    print("      최종 확인은 sniff 로 1005/1006 기지국 실좌표를 봐야 한다.")


def cmd_sniff(a):
    s = ntrip_open(a.host, a.port, a.mount, a.user, getattr(a, "pw"))
    status, head, rest = read_header(s)
    print(f"# 접속: {a.host}:{a.port}/{a.mount}")
    print(f"# 응답: {status}")
    if "200" not in status:
        print(head.decode("latin1", "replace"))
        return 1

    gga = None
    if a.gga_from_latlon:
        gga = nmea_gga(a.gga_from_latlon[0], a.gga_from_latlon[1])
        s.sendall(gga)
        print(f"# GGA 업링크: {gga.decode().strip()}")

    s.settimeout(5)
    buf = bytearray(rest)
    types = Counter()
    bases = {}          # station_id -> 마지막 좌표
    base_moves = []     # (t, sid, 이동거리 m)
    total = 0
    t0 = time.time()
    last_gga = t0

    while time.time() - t0 < a.secs:
        try:
            chunk = s.recv(4096)
        except socket.timeout:
            print("  (수신 없음 5초)")
            continue
        if not chunk:
            print("# 스트림 종료")
            break
        total += len(chunk)
        buf += chunk

        i = 0
        while True:
            j = buf.find(b"\xd3", i)
            if j < 0 or len(buf) - j < 3:
                break
            # 길이 상위 바이트의 reserved 6bit 는 항상 0. 이걸 안 보면 페이로드 안의
            # 우연한 0xD3 을 프레임 시작으로 착각해 최대 1023B 동안 파서가 멈춘다.
            if buf[j + 1] & 0xFC:
                i = j + 1
                continue
            length = ((buf[j + 1] & 0x03) << 8) | buf[j + 2]
            frame_len = 3 + length + 3
            if len(buf) - j < frame_len:
                break
            frame = bytes(buf[j:j + frame_len])
            got = int.from_bytes(frame[-3:], "big")
            if crc24q(frame[:-3]) != got:
                i = j + 1                      # 오정렬 → 다음 0xD3 부터
                continue
            payload = frame[3:3 + length]
            if length >= 2:
                mt = (payload[0] << 4) | (payload[1] >> 4)
                types[mt] += 1
                if mt in (1005, 1006):
                    p = parse_1005_1006(payload)
                    if p:
                        sid, x, y, z, h = p
                        prev = bases.get(sid)
                        if prev and (abs(prev[0] - x) > 0.5 or abs(prev[1] - y) > 0.5
                                     or abs(prev[2] - z) > 0.5):
                            d = math.dist(prev[:3], (x, y, z))
                            base_moves.append((time.time() - t0, sid, d))
                            print(f"  ⚠️ 기지국 좌표 이동: id={sid} {d:.1f} m  (t+{time.time()-t0:.0f}s)")
                        bases[sid] = (x, y, z, h)
            i = j + frame_len
        del buf[:i]

        if gga and time.time() - last_gga > 10:
            s.sendall(gga)
            last_gga = time.time()

    s.close()

    dur = time.time() - t0
    print(f"\n## 결과 ({dur:.0f}초, {total} bytes, {total/max(dur,1):.0f} B/s)")
    print("\n### RTCM 메시지 종류")
    for mt, c in sorted(types.items()):
        tag = f"  ← {NETWORK_MSGS[mt]}" if mt in NETWORK_MSGS else ""
        print(f"  {mt:<5} x{c:<6}{tag}")

    print("\n### 기지국(1005/1006) 좌표")
    if not bases:
        print("  없음 — 1005/1006 이 안 왔다. 관측 시간을 늘려보라(권장 60초 이상).")
    for sid, (x, y, z, h) in bases.items():
        lat, lon, alt = ecef_to_lla(x, y, z)
        line = f"  id={sid}  {lat:.7f}, {lon:.7f}, {alt:.2f}m"
        if h is not None:
            line += f" (안테나고 {h:.3f}m)"
        print(line)
        if a.gga_from_latlon:
            d = haversine_km(a.gga_from_latlon[0], a.gga_from_latlon[1], lat, lon)
            print(f"        베이스라인 {d:.2f} km")

    print("\n### 판정")
    net_found = sorted(set(types) & set(NETWORK_MSGS))
    if net_found:
        print(f"  ❌ 네트워크 RTK 스트림 — 네트워크 전용 메시지 {net_found} 존재")
    elif base_moves:
        print(f"  ❌ 기지국 좌표가 {len(base_moves)}회 이동 — VRS(가상 기지국)")
    elif bases:
        print("  ✅ 단일기준국으로 보임 — 네트워크 메시지 없음 + 기지국 좌표 고정")
    else:
        print("  ? 판정 불가 — 1005/1006 미수신")
    print("\n  ※ VRS 는 GGA 를 보낸 위치 근처에 가상 기지국을 만든다. 정지 상태로 짧게 보면")
    print("     좌표가 고정으로 보일 수 있으므로, 네트워크 메시지 유무를 1차 근거로 삼을 것.")
    return 0


def main():
    ap = argparse.ArgumentParser(description=__doc__,
                                 formatter_class=argparse.RawDescriptionHelpFormatter)
    sub = ap.add_subparsers(dest="cmd", required=True)

    p = sub.add_parser("table", help="소스테이블 출력")
    p.add_argument("host"); p.add_argument("port", type=int)
    p.add_argument("--filter", help="마운트포인트 이름 부분일치 필터")
    p.set_defaults(func=cmd_table)

    p = sub.add_parser("near", help="내 위치에서 가까운 기준국 정렬")
    p.add_argument("host"); p.add_argument("port", type=int)
    p.add_argument("lat", type=float); p.add_argument("lon", type=float)
    p.add_argument("-n", type=int, default=10)
    p.add_argument("--format", help="포맷 필터 (예: RTCM3.2)")
    p.set_defaults(func=cmd_near)

    p = sub.add_parser("sniff", help="스트림을 뜯어 VRS/단일기준국 판정")
    p.add_argument("host"); p.add_argument("port", type=int); p.add_argument("mount")
    p.add_argument("--user", default=""); p.add_argument("--pass", dest="pw", default="")
    p.add_argument("--gga-from-latlon", nargs=2, type=float, metavar=("LAT", "LON"),
                   help="GGA 업링크 (VRS/nmea=1 마운트포인트에 필요)")
    p.add_argument("--secs", type=int, default=60)
    p.set_defaults(func=cmd_sniff)

    a = ap.parse_args()
    sys.exit(a.func(a) or 0)


if __name__ == "__main__":
    main()