// UM982 (Unicore) driver for FAST-LIVO2-RTK (ROS 2 Humble)
// ---------------------------------------------------------------------------
// 한 노드에서:
//   1) UM982 시리얼(COM3) 오픈 — BESTNAV(binary) + GGA(ascii) 혼합 스트림 수신
//   2) NTRIP(VRS) 접속 → RTCM3 을 같은 시리얼로 주입(별도 스레드, 자동 재접속)
//   3) 첫 유효 GGA 로 VRS 부트스트랩, 이후 주기적 재전송
//   4) BESTNAV 를 파싱해 gnss_comm/msg/GnssPVTSolnMsg 로 발행 (gps.gps_topic)
//
// 시리얼 포트는 한 프로세스만 점유 가능하므로 RTCM 주입과 파싱이 반드시 한 노드에 있어야 한다.
// 파서 오프셋 근거: Unicore N4 Reference Commands Manual, §7.3.26(BESTNAV)/Table 7-49(헤더).
// 대상 필드 근거: optimization.cpp::gpsHandler (latitude/longitude/altitude, time, vel, h/v_acc).
//
// 의존성: rclcpp, gnss_comm  (시리얼=termios, NTRIP=POSIX 소켓 — 외부 라이브러리 불필요)
// 가정: 호스트가 little-endian (x86_64/aarch64 리눅스 — Unicore 바이너리도 LE).

#include <rclcpp/rclcpp.hpp>
#include <gnss_comm/msg/gnss_pvt_soln_msg.hpp>

#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <netdb.h>
#include <sys/socket.h>
#include <arpa/inet.h>

#include <algorithm>
#include <array>
#include <atomic>
#include <cmath>
#include <cstring>
#include <limits>
#include <map>
#include <mutex>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

using gnss_comm::msg::GnssPVTSolnMsg;

namespace {

// ── BESTNAV 바이너리 레이아웃 (little-endian) ──────────────────────────────
constexpr uint8_t  SYNC0 = 0xAA, SYNC1 = 0x44, SYNC2 = 0xB5;
constexpr size_t   HDR_LEN   = 24;    // 동기 3B 포함 헤더 총 길이
constexpr uint16_t BESTNAV_ID = 2118;

// 헤더 오프셋
constexpr size_t OFF_MSGID = 4, OFF_MSGLEN = 6, OFF_TSTAT = 9, OFF_WEEK = 10, OFF_MS = 12;
// 본문(절대) 오프셋 = HDR_LEN + 표(§3.3) 상대오프셋
constexpr size_t OFF_POSTYPE = 28;
constexpr size_t OFF_LAT = 32, OFF_LON = 40, OFF_HGT = 48, OFF_UNDU = 56;
constexpr size_t OFF_LATSTD = 64, OFF_LONSTD = 68, OFF_HGTSTD = 72;
constexpr size_t OFF_DIFFAGE = 80, OFF_NSOLN = 89;
constexpr size_t OFF_HORSPD = 112, OFF_TRKGND = 120, OFF_VERTSPD = 128, OFF_HORSPDSTD = 140;

// Position/Velocity Type (Table 0-4)
constexpr int32_t POS_NONE = 0, POS_SINGLE = 16;
inline bool is_rtk_fixed(int32_t t) { return t == 48 || t == 49 || t == 50; }
inline bool is_rtk_float(int32_t t) { return t == 32 || t == 33 || t == 34; }

template <typename T>
inline T rd(const uint8_t *p, size_t off) { T v; std::memcpy(&v, p + off, sizeof(T)); return v; }

// NovAtel/Unicore 32-bit CRC (Appendix 1)
uint32_t crc32_value(int i) {
  uint32_t crc = static_cast<uint32_t>(i);
  for (int j = 8; j > 0; --j)
    crc = (crc & 1) ? (crc >> 1) ^ 0xEDB88320UL : (crc >> 1);
  return crc;
}
uint32_t block_crc32(const uint8_t *buf, size_t n) {
  uint32_t crc = 0;
  while (n-- != 0) {
    uint32_t t1 = (crc >> 8) & 0x00FFFFFFUL;
    uint32_t t2 = crc32_value((static_cast<int>(crc) ^ *buf++) & 0xFF);
    crc = t1 ^ t2;
  }
  return crc;
}

// ── RTCM3 스니핑 (주입 스트림 검증) ─────────────────────────────────────────
// 프레임: 0xD3 | 6bit reserved + 10bit len | payload | CRC-24Q 3B
// 목적 두 가지:
//  (1) 붙은 마운트포인트가 실제 단일기준국인지 — 1005/1006 좌표 고정 + 네트워크 메시지 부재
//  (2) UM982 가 실제로 쓸 수 있는 위성군 보정이 오는지 (아래 rtcm_documented 참고)
const std::array<uint32_t, 256> &crc24q_table() {
  static const std::array<uint32_t, 256> t = [] {
    std::array<uint32_t, 256> a{};
    for (int i = 0; i < 256; ++i) {
      uint32_t crc = static_cast<uint32_t>(i) << 16;
      for (int j = 0; j < 8; ++j) {
        crc <<= 1;
        if (crc & 0x1000000) crc ^= 0x1864CFBUL;
      }
      a[i] = crc & 0xFFFFFF;
    }
    return a;
  }();
  return t;
}

uint32_t crc24q(const uint8_t *buf, size_t n) {
  const auto &tab = crc24q_table();
  uint32_t crc = 0;
  for (size_t i = 0; i < n; ++i)
    crc = ((crc << 8) & 0xFFFFFF) ^ tab[((crc >> 16) ^ buf[i]) & 0xFF];
  return crc;
}

// MSB-first 비트 리더 (RTCM DF 필드)
struct Bits {
  const uint8_t *b;
  size_t pos{0};
  explicit Bits(const uint8_t *p) : b(p) {}
  uint64_t u(int n) {
    uint64_t v = 0;
    for (int i = 0; i < n; ++i) { v = (v << 1) | ((b[pos >> 3] >> (7 - (pos & 7))) & 1); ++pos; }
    return v;
  }
  int64_t s(int n) {
    uint64_t v = u(n);
    return (v & (1ULL << (n - 1))) ? static_cast<int64_t>(v) - (1LL << n) : static_cast<int64_t>(v);
  }
};

struct BasePos { int id; double x, y, z, ant_h; bool has_ant_h; };

// 1005(ARP) / 1006(ARP + 안테나고) → ECEF(m)
bool parse_base_pos(const uint8_t *pl, size_t len, BasePos &out) {
  if (len < 19) return false;
  Bits b(pl);
  int msg = static_cast<int>(b.u(12));
  if (msg != 1005 && msg != 1006) return false;
  if (msg == 1006 && len < 21) return false;
  out.id = static_cast<int>(b.u(12));
  b.u(6);                              // ITRF realization year
  b.u(1); b.u(1); b.u(1); b.u(1);      // GPS/GLO/GAL indicator, reference station indicator
  out.x = static_cast<double>(b.s(38)) * 1e-4;
  b.u(1); b.u(1);                      // single receiver osc, reserved
  out.y = static_cast<double>(b.s(38)) * 1e-4;
  b.u(2);                              // quarter cycle indicator
  out.z = static_cast<double>(b.s(38)) * 1e-4;
  out.has_ant_h = (msg == 1006);
  out.ant_h = out.has_ant_h ? static_cast<double>(b.u(16)) * 1e-4 : 0.0;
  return true;
}

void ecef_to_lla(double x, double y, double z, double &lat, double &lon, double &alt) {
  constexpr double a = 6378137.0, f = 1.0 / 298.257223563;
  constexpr double e2 = f * (2.0 - f);
  lon = std::atan2(y, x);
  double p = std::hypot(x, y);
  lat = std::atan2(z, p * (1.0 - e2));
  double n = a;
  for (int i = 0; i < 10; ++i) {
    n = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat));
    alt = p / std::cos(lat) - n;
    lat = std::atan2(z, p * (1.0 - e2 * n / (n + alt)));
  }
  n = a / std::sqrt(1.0 - e2 * std::sin(lat) * std::sin(lat));
  alt = p / std::cos(lat) - n;
  lat *= 180.0 / M_PI;
  lon *= 180.0 / M_PI;
}

double haversine_km(double lat1, double lon1, double lat2, double lon2) {
  constexpr double R = 6371.0, D = M_PI / 180.0;
  double dlat = (lat2 - lat1) * D, dlon = (lon2 - lon1) * D;
  double h = std::sin(dlat / 2) * std::sin(dlat / 2)
           + std::cos(lat1 * D) * std::cos(lat2 * D) * std::sin(dlon / 2) * std::sin(dlon / 2);
  return 2 * R * std::asin(std::sqrt(std::min(1.0, h)));
}

// 버퍼에서 CRC 가 맞는 완성 프레임만 뽑아 콜백에 넘기고, 소비분을 앞에서 제거한다.
// on_frame(msg_type, payload, payload_len). 미완성 프레임은 버퍼에 남겨 다음 recv 를 기다린다.
template <typename F>
void extract_rtcm_frames(std::vector<uint8_t> &buf, F &&on_frame) {
  size_t i = 0;
  while (true) {
    size_t j = i;
    while (j < buf.size() && buf[j] != 0xD3) ++j;
    if (j + 3 > buf.size()) { i = j; break; }
    // 길이 상위 바이트의 reserved 6bit 는 RTCM3 에서 항상 0. 이걸 안 보면 관측치 페이로드
    // 안의 우연한 0xD3 을 프레임 시작으로 착각해, 그 가짜 길이만큼(최대 1023B) 데이터가
    // 더 쌓일 때까지 파서가 멈춘다.
    if ((buf[j + 1] & 0xFC) != 0) { i = j + 1; continue; }
    size_t len = (static_cast<size_t>(buf[j + 1] & 0x03) << 8) | buf[j + 2];
    size_t total = 3 + len + 3;
    if (buf.size() - j < total) { i = j; break; }
    const uint8_t *f = buf.data() + j;
    uint32_t got = (static_cast<uint32_t>(f[total - 3]) << 16)
                 | (static_cast<uint32_t>(f[total - 2]) << 8)
                 | static_cast<uint32_t>(f[total - 1]);
    if (crc24q(f, total - 3) != got) { i = j + 1; continue; }   // 오정렬 → 다음 0xD3 부터
    if (len >= 2) on_frame((static_cast<int>(f[3]) << 4) | (f[4] >> 4), f + 3, len);
    i = j + total;
  }
  buf.erase(buf.begin(), buf.begin() + i);
}

// 네트워크 RTK(VRS/FKP/MAC) 전용 메시지 — 하나라도 있으면 단일기준국이 아니다
const char *network_msg_name(int t) {
  switch (t) {
    case 1014: return "Network Aux Station(MAC)";
    case 1015: case 1016: case 1017: return "MAC correction diff";
    case 1030: case 1031: return "Network RTK residual";
    case 1032: return "Physical Ref Station Pos(VRS)";
    case 1034: case 1035: return "FKP gradient";
    default: return nullptr;
  }
}

const char *msm_constellation(int t) {
  if (t >= 1071 && t <= 1077) return "GPS";
  if (t >= 1081 && t <= 1087) return "GLONASS";
  if (t >= 1091 && t <= 1097) return "Galileo";
  if (t >= 1101 && t <= 1107) return "SBAS";
  if (t >= 1111 && t <= 1117) return "QZSS";
  if (t >= 1121 && t <= 1127) return "BeiDou";
  return nullptr;
}

// Unicore N4 매뉴얼 Appendix 2 "Supported RTCM V3 messages" 기재분.
// ⚠️ Galileo MSM(1091~1097)과 QZSS MSM(1111~1117)은 매뉴얼에 없다. 이 메시지로만 오는
//    위성군 보정은 수신기가 못 쓸 수 있고, 그러면 RTK 에 실제로 기여하는 위성이 확 준다.
bool rtcm_documented(int t) {
  if (t >= 1001 && t <= 1004) return true;   // GPS L1/L2
  if (t >= 1009 && t <= 1012) return true;   // GLONASS L1/L2
  if (t == 1074 || t == 1075) return true;   // GPS MSM4/5
  if (t == 1084 || t == 1085) return true;   // GLONASS MSM4/5
  if (t >= 1123 && t <= 1127) return true;   // BDS MSM3~7
  if (t == 1104) return true;                // BDS observables (Unicore)
  if (t == 1005 || t == 1006 || t == 1007 || t == 1033) return true;
  if (t == 1019 || t == 1020 || t == 1042 || t == 1045 || t == 1046) return true;
  return false;
}

speed_t to_speed(int baud) {
  switch (baud) {
    case 9600: return B9600;      case 19200: return B19200;
    case 38400: return B38400;    case 57600: return B57600;
    case 115200: return B115200;  case 230400: return B230400;
    case 460800: return B460800;  case 921600: return B921600;
    default: return B460800;
  }
}

std::string base64(const std::string &in) {
  static const char *T = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
  std::string out;
  int val = 0, bits = -6;
  for (unsigned char c : in) {
    val = (val << 8) + c; bits += 8;
    while (bits >= 0) { out.push_back(T[(val >> bits) & 0x3F]); bits -= 6; }
  }
  if (bits > -6) out.push_back(T[((val << 8) >> (bits + 8)) & 0x3F]);
  while (out.size() % 4) out.push_back('=');
  return out;
}

}  // namespace

class Um982Driver : public rclcpp::Node {
 public:
  Um982Driver() : Node("um982_driver") {
    // ── 파라미터 ──
    serial_port_ = declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
    serial_baud_ = declare_parameter<int>("serial_baud", 460800);
    gps_topic_   = declare_parameter<std::string>("gps_topic", "/ublox_driver/receiver_pvt");
    ntrip_host_  = declare_parameter<std::string>("ntrip_host", "RTS1.ngii.go.kr");
    ntrip_port_  = declare_parameter<int>("ntrip_port", 2101);
    ntrip_user_  = declare_parameter<std::string>("ntrip_user", "");
    ntrip_pass_  = declare_parameter<std::string>("ntrip_pass", "");
    ntrip_mp_    = declare_parameter<std::string>("ntrip_mountpoint", "VRS-RTCM34");
    use_ntrip_   = declare_parameter<bool>("use_ntrip", true);
    fixed_only_  = declare_parameter<bool>("publish_fixed_only", false);
    inflate_cov_ = declare_parameter<bool>("inflate_cov_when_not_fixed", true);
    max_corr_age_ = declare_parameter<double>("max_corr_age", 30.0);  // 보정 나이 초과 시 코스팅으로 간주
    gga_period_  = declare_parameter<double>("gga_send_period", 10.0);
    // ntrip_mountpoint 가 "AUTO" 면 소스테이블을 받아 현재 위치에서 가장 가까운
    // 단일기준국(solution=0)을 고른다. 아래는 그 선택 기준.
    auto_format_ = declare_parameter<std::string>("ntrip_auto_format", "RTCM 3.2");
    max_baseline_km_ = declare_parameter<double>("max_baseline_km", 30.0);  // 초과 시 경고만

    if (ntrip_mp_ != "AUTO" && ntrip_mp_ != "auto") active_mp_ = ntrip_mp_;

    pub_ = create_publisher<GnssPVTSolnMsg>(gps_topic_, rclcpp::QoS(rclcpp::KeepLast(2000)));

    if (!open_serial()) {
      RCLCPP_FATAL(get_logger(), "시리얼 오픈 실패: %s", serial_port_.c_str());
      throw std::runtime_error("serial open failed");
    }
    RCLCPP_INFO(get_logger(), "UM982 %s @ %d → 발행 %s",
                serial_port_.c_str(), serial_baud_, gps_topic_.c_str());

    // RAM 에만 적용되는 로그 활성화(SAVECONFIG 안 하므로 재부팅 시 원복). 수신기가 어떤 RTCM 을
    // 실제로 받아들이는지 보려면 이게 켜져 있어야 한다.
    if (declare_parameter<bool>("enable_rtcm_status_log", true)) {
      static constexpr char kCmd[] = "RTCMSTATUSA ONCHANGED\r\n";
      ::write(serial_fd_, kCmd, sizeof(kCmd) - 1);
    }

    running_ = true;
    serial_thread_ = std::thread(&Um982Driver::serial_loop, this);
    if (use_ntrip_) ntrip_thread_ = std::thread(&Um982Driver::ntrip_loop, this);
  }

  ~Um982Driver() override {
    running_ = false;
    if (serial_thread_.joinable()) serial_thread_.join();
    if (ntrip_thread_.joinable()) ntrip_thread_.join();
    if (serial_fd_ >= 0) ::close(serial_fd_);
    if (ntrip_fd_ >= 0) ::close(ntrip_fd_);
  }

 private:
  // ── 시리얼 ──────────────────────────────────────────────────────────────
  bool open_serial() {
    serial_fd_ = ::open(serial_port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (serial_fd_ < 0) return false;
    termios tio{};
    if (tcgetattr(serial_fd_, &tio) != 0) return false;
    cfmakeraw(&tio);
    cfsetispeed(&tio, to_speed(serial_baud_));
    cfsetospeed(&tio, to_speed(serial_baud_));
    tio.c_cflag |= (CLOCAL | CREAD);
    tio.c_cflag &= ~CRTSCTS;
    tio.c_cc[VMIN] = 0;
    tio.c_cc[VTIME] = 0;
    return tcsetattr(serial_fd_, TCSANOW, &tio) == 0;
  }

  void serial_loop() {
    std::vector<uint8_t> buf;
    buf.reserve(1 << 16);
    uint8_t tmp[4096];
    while (running_ && rclcpp::ok()) {
      ssize_t n = ::read(serial_fd_, tmp, sizeof(tmp));
      if (n > 0) buf.insert(buf.end(), tmp, tmp + n);
      else { std::this_thread::sleep_for(std::chrono::milliseconds(5)); }
      demux(buf);
      if (buf.size() > (1 << 18)) buf.erase(buf.begin(), buf.end() - 256);  // 안전장치
    }
  }

  // BESTNAV(binary) 프레임과 GGA(ascii) 라인을 분리
  void demux(std::vector<uint8_t> &buf) {
    size_t search = 0;
    while (true) {
      // 다음 동기 위치
      size_t sync = find_sync(buf, search);
      // 동기 이전(head)에서 GGA 추출
      size_t head_end = (sync == std::string::npos) ? buf.size() : sync;
      scan_gga(buf, search, head_end);
      scan_rtcm_status(buf, search, head_end);

      if (sync == std::string::npos) {
        if (buf.size() > 2) buf.erase(buf.begin(), buf.end() - 2);  // 동기 경계 보존
        return;
      }
      if (buf.size() - sync < HDR_LEN) { buf.erase(buf.begin(), buf.begin() + sync); return; }
      uint16_t mlen = rd<uint16_t>(buf.data(), sync + OFF_MSGLEN);
      size_t total = HDR_LEN + mlen + 4;
      if (buf.size() - sync < total) { buf.erase(buf.begin(), buf.begin() + sync); return; }

      const uint8_t *f = buf.data() + sync;
      if (rd<uint16_t>(f, OFF_MSGID) == BESTNAV_ID) {
        uint32_t got = rd<uint32_t>(f, HDR_LEN + mlen);
        if (block_crc32(f, HDR_LEN + mlen) == got) handle_bestnav(f);
        else RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "BESTNAV CRC 불일치 — 스킵");
      }
      buf.erase(buf.begin(), buf.begin() + sync + total);
      search = 0;
    }
  }

  static size_t find_sync(const std::vector<uint8_t> &b, size_t from) {
    for (size_t i = from; i + 2 < b.size(); ++i)
      if (b[i] == SYNC0 && b[i + 1] == SYNC1 && b[i + 2] == SYNC2) return i;
    return std::string::npos;
  }

  // buf[begin,end) 구간에서 $G?GGA 라인을 찾아 최신 GGA(fix 유효분) 갱신
  void scan_gga(const std::vector<uint8_t> &buf, size_t begin, size_t end) {
    for (size_t p = begin; p + 4 < end; ++p) {
      if (buf[p] != '$') continue;
      if (!(p + 6 < end && buf[p + 3] == 'G' && buf[p + 4] == 'G' && buf[p + 5] == 'A')) continue;
      size_t e = p;
      while (e < end && buf[e] != '\r' && buf[e] != '\n') ++e;
      if (e >= end) break;  // 종결문자 없음 = 라인 미완 → 다음 read 까지 대기(잘린 GGA 전송 방지)
      std::string line(reinterpret_cast<const char *>(&buf[p]), e - p);
      // field 6 = fix quality
      int comma = 0; size_t i = 0; std::string fix;
      for (; i < line.size(); ++i) {
        if (line[i] == ',') { if (++comma == 6) { size_t s = i + 1; while (s < line.size() && line[s] != ',') fix.push_back(line[s++]); break; } }
      }
      if (!fix.empty() && fix != "0") {
        std::lock_guard<std::mutex> lk(gga_mtx_);
        latest_gga_ = line + "\r\n";
      }
      p = e;
    }
  }

  // 수신기가 "받아들인" RTCM 을 보여주는 진단 로그(#RTCMSTATUSA, 매뉴얼 §7.3.56).
  // 캐스터가 "보낸" 것(log_rtcm_summary)과 대조하면, 보냈는데 수신기가 안 쓰는 위성군이 드러난다.
  // 출력 형식: #RTCMSTATUSA,...;<MsgID>,<MsgNum>,<BaseID>,<SatsNum>,<L1..L6>*<CRC>
  void scan_rtcm_status(const std::vector<uint8_t> &buf, size_t begin, size_t end) {
    static constexpr char kTag[] = "#RTCMSTATUSA";
    constexpr size_t kTagLen = sizeof(kTag) - 1;
    for (size_t p = begin; p + kTagLen < end; ++p) {
      if (buf[p] != '#') continue;
      if (std::memcmp(&buf[p], kTag, kTagLen) != 0) continue;
      size_t e = p;
      while (e < end && buf[e] != '\r' && buf[e] != '\n') ++e;
      if (e >= end) break;                       // 라인 미완 → 다음 read 대기
      std::string line(reinterpret_cast<const char *>(&buf[p]), e - p);
      size_t semi = line.find(';');
      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 10000, "수신기 RTCM 수용: %s",
                           semi == std::string::npos ? line.c_str() : line.c_str() + semi + 1);
      p = e;
    }
  }

  void handle_bestnav(const uint8_t *f) {
    int32_t pos_type = rd<int32_t>(f, OFF_POSTYPE);
    if (fixed_only_ && !is_rtk_fixed(pos_type)) return;

    GnssPVTSolnMsg m;
    m.time.week = rd<uint16_t>(f, OFF_WEEK);
    m.time.tow  = rd<uint32_t>(f, OFF_MS) / 1000.0;

    double lat = rd<double>(f, OFF_LAT);
    double lon = rd<double>(f, OFF_LON);
    double hgt = rd<double>(f, OFF_HGT);          // MSL
    float  undu = rd<float>(f, OFF_UNDU);
    m.latitude = lat;
    m.longitude = lon;
    if (pos_type != POS_NONE) { rover_lat_.store(lat); rover_lon_.store(lon); }
    m.altitude = hgt + undu;                       // 타원체고 (백엔드 ENU 입력)
    m.height_msl = hgt;

    float lat_s = rd<float>(f, OFF_LATSTD);
    float lon_s = rd<float>(f, OFF_LONSTD);
    float hgt_s = rd<float>(f, OFF_HGTSTD);
    float diff_age = rd<float>(f, OFF_DIFFAGE);   // 보정 나이(s). 커지면 코스팅 중
    m.h_acc = std::hypot(lat_s, lon_s);
    m.v_acc = hgt_s;

    double hor = rd<double>(f, OFF_HORSPD);
    double trk = rd<double>(f, OFF_TRKGND) * M_PI / 180.0;
    double vert = rd<double>(f, OFF_VERTSPD);
    m.vel_n = hor * std::cos(trk);
    m.vel_e = hor * std::sin(trk);
    m.vel_d = -vert;                               // BESTNAV vert(+up) → NED down
    m.vel_acc = rd<float>(f, OFF_HORSPDSTD);

    m.num_sv = f[OFF_NSOLN];
    m.fix_type = (pos_type == POS_NONE) ? 0 : 3;
    m.valid_fix = (pos_type != POS_NONE);
    m.diff_soln = (pos_type != POS_NONE && pos_type != POS_SINGLE);
    m.carr_soln = is_rtk_fixed(pos_type) ? 2 : (is_rtk_float(pos_type) ? 1 : 0);
    m.p_dop = 0.0;

    // 신뢰 낮은 해는 공분산 부풀려 백엔드 가중 약화(백엔드에 품질 필터 없음):
    //  (1) RTK Fixed 가 아니거나, (2) 보정이 끊겨 코스팅 중(diff_age 과다)
    bool coasting = (max_corr_age_ > 0.0 && diff_age > max_corr_age_);
    if (inflate_cov_ && (m.carr_soln != 2 || coasting)) {
      m.h_acc = std::max<double>(m.h_acc, 0.5);
      m.v_acc = std::max<double>(m.v_acc, 1.0);
    }

    pub_->publish(m);

    if (pos_type != last_type_) {
      RCLCPP_INFO(get_logger(), "상태: %s(pos_type=%d) | lat=%.7f lon=%.7f alt=%.2f hσ=%.3fm nSV=%u diff_age=%.1fs",
                  is_rtk_fixed(pos_type) ? "RTK FIXED" : is_rtk_float(pos_type) ? "RTK FLOAT"
                  : pos_type == POS_SINGLE ? "SINGLE" : pos_type == POS_NONE ? "NONE" : "OTHER",
                  pos_type, lat, lon, m.altitude, m.h_acc, m.num_sv, diff_age);
      last_type_ = pos_type;
    }
  }

  // ── NTRIP ───────────────────────────────────────────────────────────────
  void ntrip_loop() {
    const bool auto_mp = (ntrip_mp_ == "AUTO" || ntrip_mp_ == "auto");
    const bool send_gga = (gga_period_ > 0.0);

    // GGA 대기가 필요한 경우는 둘뿐:
    //  · VRS/네트워크 마운트포인트 — GGA 를 보내야 스트림이 시작된다
    //  · AUTO — 가장 가까운 기준국을 고르려면 내 위치를 알아야 한다
    // 단일기준국(nmea=0)에 고정 마운트포인트면 GGA 없이 즉시 붙어야 한다. 안 그러면
    // 수신기가 자력으로 fix 를 얻기 전까지 RTCM 이 한 바이트도 안 들어간다.
    if (send_gga || auto_mp) {
      RCLCPP_INFO(get_logger(), "NTRIP: 첫 GGA 대기 중%s", auto_mp ? " (AUTO 기준국 선택용)" : "");
      while (running_ && rclcpp::ok() && latest_gga().empty())
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    } else {
      RCLCPP_INFO(get_logger(), "NTRIP: GGA 업링크 없음(단일기준국) → 즉시 접속");
    }
    if (!running_ || !rclcpp::ok()) return;

    if (auto_mp && !resolve_auto_mountpoint())
      RCLCPP_ERROR(get_logger(), "AUTO 기준국 선택 실패 — 재시도한다");

    auto last_send = std::chrono::steady_clock::now();
    auto last_summary = std::chrono::steady_clock::now();
    while (running_ && rclcpp::ok()) {
      if (ntrip_fd_ < 0) {
        if (auto_mp && active_mp_.empty() && !resolve_auto_mountpoint()) {
          std::this_thread::sleep_for(std::chrono::seconds(5));
          continue;
        }
        if (!ntrip_connect()) {
          // AUTO 면 다음 후보로 넘어간다(해당 관측소 점검/장애 대비)
          if (auto_mp && !next_candidate())
            RCLCPP_WARN(get_logger(), "후보 소진 → 소스테이블 재조회");
          std::this_thread::sleep_for(std::chrono::seconds(2));
          continue;
        }
        RCLCPP_INFO(get_logger(), "NTRIP 접속: %s:%d/%s", ntrip_host_.c_str(), ntrip_port_, active_mp_.c_str());
        last_send = std::chrono::steady_clock::now();
      }
      uint8_t buf[4096];
      ssize_t n = ::recv(ntrip_fd_, buf, sizeof(buf), 0);
      if (n > 0) {
        ::write(serial_fd_, buf, n);         // RTCM 주입
        ntrip_bytes_total_ += n;
        feed_rtcm(buf, static_cast<size_t>(n));   // 주입과 병행해 스트림 검증
      } else if (n == 0) {
        RCLCPP_WARN(get_logger(), "NTRIP 연결 종료 → 재접속");
        ::close(ntrip_fd_); ntrip_fd_ = -1; continue;
      }
      auto now = std::chrono::steady_clock::now();
      // 무엇이 실제로 들어오는지 30초마다 1줄 요약 (위성군 보정 누락 진단의 핵심)
      if (std::chrono::duration<double>(now - last_summary).count() > 30.0) {
        log_rtcm_summary();
        last_summary = now;
      }
      // VRS 유지용 GGA 주기 재전송
      if (send_gga && std::chrono::duration<double>(now - last_send).count() > gga_period_) {
        std::string g = latest_gga();
        if (!g.empty()) ::send(ntrip_fd_, g.data(), g.size(), MSG_NOSIGNAL);
        last_send = now;
      }
    }
  }

  // ── AUTO: 소스테이블 → 최근접 단일기준국 ────────────────────────────────
  // "주변에 있는 진짜 기지국을 우선으로 잡는다" 를 그대로 구현. solution 필드가 0 인
  // STR(=단일기준국)만 후보로 두고, 소스테이블의 위경도로 거리순 정렬한다.
  bool resolve_auto_mountpoint() {
    double rlat = rover_lat_.load(), rlon = rover_lon_.load();
    if (std::isnan(rlat)) {
      RCLCPP_WARN(get_logger(), "AUTO: 로버 위치 미확보 — 기준국 선택 보류");
      return false;
    }
    std::string table;
    if (!fetch_sourcetable(table)) {
      RCLCPP_ERROR(get_logger(), "AUTO: 소스테이블 조회 실패");
      return false;
    }
    std::vector<std::pair<double, std::string>> cand;
    std::istringstream ss(table);
    std::string line;
    while (std::getline(ss, line)) {
      if (line.rfind("STR;", 0) != 0) continue;
      std::vector<std::string> f;
      std::string tok;
      std::istringstream ls(line);
      while (std::getline(ls, tok, ';')) f.push_back(tok);
      if (f.size() < 13) continue;
      if (f[12] != "0") continue;                        // solution=0 → 단일기준국만
      if (!auto_format_.empty() && f[3] != auto_format_) continue;
      double blat, blon;
      try { blat = std::stod(f[9]); blon = std::stod(f[10]); } catch (...) { continue; }
      if (blat == 0.0 && blon == 0.0) continue;
      cand.emplace_back(haversine_km(rlat, rlon, blat, blon), f[1]);
    }
    if (cand.empty()) {
      RCLCPP_ERROR(get_logger(), "AUTO: 조건(solution=0, format=%s)에 맞는 기준국 없음", auto_format_.c_str());
      return false;
    }
    std::sort(cand.begin(), cand.end());
    candidates_.clear();
    for (size_t i = 0; i < cand.size() && i < 5; ++i) candidates_.push_back(cand[i].second);
    cand_idx_ = 0;
    active_mp_ = candidates_.front();
    std::string others;
    for (size_t i = 1; i < candidates_.size(); ++i) others += " " + candidates_[i];
    RCLCPP_INFO(get_logger(), "AUTO 기준국 선택: %s (%.1f km) | 예비:%s",
                active_mp_.c_str(), cand[0].first, others.empty() ? " 없음" : others.c_str());
    RCLCPP_INFO(get_logger(), "  ※ 소스테이블 좌표는 반올림/오기입이 있다. 실제 거리는 아래 1005/1006 로그로 확인할 것");
    return true;
  }

  bool next_candidate() {
    if (candidates_.empty() || cand_idx_ + 1 >= candidates_.size()) { active_mp_.clear(); return false; }
    active_mp_ = candidates_[++cand_idx_];
    RCLCPP_WARN(get_logger(), "AUTO: 다음 후보로 전환 → %s", active_mp_.c_str());
    return true;
  }

  bool fetch_sourcetable(std::string &out) {
    addrinfo hints{}, *res = nullptr;
    hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM;
    if (getaddrinfo(ntrip_host_.c_str(), std::to_string(ntrip_port_).c_str(), &hints, &res) != 0) return false;
    int fd = ::socket(res->ai_family, res->ai_socktype, res->ai_protocol);
    timeval tv{10, 0};
    setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
    bool ok = (::connect(fd, res->ai_addr, res->ai_addrlen) == 0);
    freeaddrinfo(res);
    if (!ok) { ::close(fd); return false; }
    // 소스테이블 조회는 인증 불필요
    std::string req = "GET / HTTP/1.0\r\nUser-Agent: NTRIP um982_driver/1.0\r\n"
                      "Accept: */*\r\nConnection: close\r\n\r\n";
    ::send(fd, req.data(), req.size(), MSG_NOSIGNAL);
    out.clear();
    char buf[4096];
    ssize_t r;
    while ((r = ::recv(fd, buf, sizeof(buf), 0)) > 0) {
      out.append(buf, static_cast<size_t>(r));
      if (out.size() > (1u << 22)) break;   // 4MB 안전장치
    }
    ::close(fd);
    return out.find("STR;") != std::string::npos;
  }

  // ── RTCM 스니핑 ─────────────────────────────────────────────────────────
  void feed_rtcm(const uint8_t *data, size_t n) {
    rtcm_buf_.insert(rtcm_buf_.end(), data, data + n);
    extract_rtcm_frames(rtcm_buf_, [this](int mt, const uint8_t *pl, size_t len) {
      ++rtcm_types_[mt];
      if (mt == 1005 || mt == 1006) {
        BasePos bp;
        if (parse_base_pos(pl, len, bp)) note_base(bp);
      }
    });
    if (rtcm_buf_.size() > (1u << 16)) rtcm_buf_.clear();   // 안전장치
  }

  void note_base(const BasePos &bp) {
    bool moved = base_known_ && (std::fabs(bp.x - base_x_) > 0.5 ||
                                 std::fabs(bp.y - base_y_) > 0.5 ||
                                 std::fabs(bp.z - base_z_) > 0.5);
    if (base_known_ && !moved) return;
    double dx = bp.x - base_x_, dy = bp.y - base_y_, dz = bp.z - base_z_;
    double move_m = moved ? std::sqrt(dx * dx + dy * dy + dz * dz) : 0.0;
    base_x_ = bp.x; base_y_ = bp.y; base_z_ = bp.z; base_known_ = true;

    double blat, blon, balt;
    ecef_to_lla(bp.x, bp.y, bp.z, blat, blon, balt);
    double rlat = rover_lat_.load(), rlon = rover_lon_.load();
    double km = std::isnan(rlat) ? -1.0 : haversine_km(rlat, rlon, blat, blon);

    if (moved) {
      // 실제 관측소는 움직이지 않는다. 이게 뜨면 가상 기지국(VRS)이다.
      RCLCPP_WARN(get_logger(), "⚠️ 기지국 좌표 이동 %.1fm → 가상 기지국(VRS) 스트림. id=%d", move_m, bp.id);
    }
    if (km >= 0.0) {
      RCLCPP_INFO(get_logger(), "기지국 id=%d  %.7f, %.7f, %.2fm%s | 베이스라인 %.2f km",
                  bp.id, blat, blon, balt, bp.has_ant_h ? " (안테나고 포함)" : "", km);
      if (km > max_baseline_km_)
        RCLCPP_WARN(get_logger(), "⚠️ 베이스라인 %.1f km > %.1f km — 단일기준국은 거리가 멀수록 Fixed 가 어려워진다",
                    km, max_baseline_km_);
    } else {
      RCLCPP_INFO(get_logger(), "기지국 id=%d  %.7f, %.7f, %.2fm", bp.id, blat, blon, balt);
    }
  }

  // 30초마다: 어떤 RTCM 이 오는지 + 수신기가 쓸 수 있는 형태인지
  void log_rtcm_summary() {
    if (rtcm_types_.empty()) {
      RCLCPP_WARN(get_logger(), "RTCM 미수신 (누적 %ld bytes)", ntrip_bytes_total_);
      return;
    }
    std::ostringstream all, net, undoc, sats;
    for (const auto &kv : rtcm_types_) {
      all << " " << kv.first << "x" << kv.second;
      if (const char *nm = network_msg_name(kv.first)) net << " " << kv.first << "(" << nm << ")";
      if (const char *c = msm_constellation(kv.first)) {
        sats << " " << c;
        if (!rtcm_documented(kv.first)) undoc << " " << kv.first << "(" << c << ")";
      }
    }
    RCLCPP_INFO(get_logger(), "RTCM 누적 %ld bytes | 메시지:%s", ntrip_bytes_total_, all.str().c_str());
    if (!sats.str().empty())
      RCLCPP_INFO(get_logger(), "  보정 오는 위성군:%s", sats.str().c_str());
    if (!net.str().empty())
      RCLCPP_WARN(get_logger(), "  네트워크 RTK 메시지 존재 → 단일기준국 아님:%s", net.str().c_str());
    if (!undoc.str().empty())
      RCLCPP_WARN(get_logger(), "  ⚠️ 매뉴얼 미기재 메시지:%s — 이 위성군은 RTK 에 안 쓰일 수 있다", undoc.str().c_str());
  }

  bool ntrip_connect() {
    addrinfo hints{}, *res = nullptr;
    hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM;
    if (getaddrinfo(ntrip_host_.c_str(), std::to_string(ntrip_port_).c_str(), &hints, &res) != 0) return false;
    int fd = ::socket(res->ai_family, res->ai_socktype, res->ai_protocol);
    timeval tv{5, 0};
    setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
    bool ok = (::connect(fd, res->ai_addr, res->ai_addrlen) == 0);
    freeaddrinfo(res);
    if (!ok) { ::close(fd); return false; }

    std::string auth = base64(ntrip_user_ + ":" + ntrip_pass_);
    std::string req = "GET /" + active_mp_ + " HTTP/1.0\r\n"
                      "User-Agent: NTRIP um982_driver/1.0\r\nAccept: */*\r\n"
                      "Connection: close\r\nAuthorization: Basic " + auth + "\r\n\r\n";
    ::send(fd, req.data(), req.size(), MSG_NOSIGNAL);
    char resp[1024]; ssize_t r = ::recv(fd, resp, sizeof(resp) - 1, 0);
    if (r <= 0) { ::close(fd); return false; }
    resp[r] = 0;
    if (!strstr(resp, "200 OK") && !strstr(resp, "ICY 200 OK")) {
      // 401(계정 문제)과 404(마운트포인트 오타/미개방)를 구분해줘야 삽질이 없다
      std::string head(resp, std::min<size_t>(static_cast<size_t>(r), 120));
      size_t eol = head.find('\r');
      RCLCPP_ERROR(get_logger(), "NTRIP 거부 [%s] ← %s:%d/%s",
                   head.substr(0, eol == std::string::npos ? head.size() : eol).c_str(),
                   ntrip_host_.c_str(), ntrip_port_, active_mp_.c_str());
      ::close(fd);
      return false;
    }
    // 단일기준국(nmea=0)은 GGA 를 안 보내도 스트림이 나온다
    if (gga_period_ > 0.0) {
      std::string g = latest_gga();
      if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL);
    }
    rtcm_buf_.clear();
    ntrip_fd_ = fd;
    return true;
  }

  std::string latest_gga() { std::lock_guard<std::mutex> lk(gga_mtx_); return latest_gga_; }

  // 파라미터
  std::string serial_port_, gps_topic_, ntrip_host_, ntrip_user_, ntrip_pass_, ntrip_mp_;
  std::string auto_format_;
  int serial_baud_{}, ntrip_port_{};
  bool use_ntrip_{}, fixed_only_{}, inflate_cov_{};
  double gga_period_{}, max_corr_age_{}, max_baseline_km_{};

  rclcpp::Publisher<GnssPVTSolnMsg>::SharedPtr pub_;
  int serial_fd_{-1};
  std::atomic<int> ntrip_fd_{-1};
  long ntrip_bytes_total_{0};
  std::atomic<bool> running_{false};
  std::thread serial_thread_, ntrip_thread_;
  std::mutex gga_mtx_;
  std::string latest_gga_;
  int32_t last_type_{-999};

  // 로버 위치 — AUTO 기준국 선택과 베이스라인 계산용 (시리얼 스레드가 쓰고 NTRIP 스레드가 읽음)
  std::atomic<double> rover_lat_{std::numeric_limits<double>::quiet_NaN()};
  std::atomic<double> rover_lon_{std::numeric_limits<double>::quiet_NaN()};

  // NTRIP 스레드 전용 (락 불필요)
  std::string active_mp_;                    // 실제로 접속 중인 마운트포인트
  std::vector<std::string> candidates_;      // AUTO 후보(거리순)
  size_t cand_idx_{0};
  std::vector<uint8_t> rtcm_buf_;
  std::map<int, long> rtcm_types_;
  bool base_known_{false};
  double base_x_{0}, base_y_{0}, base_z_{0};
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Um982Driver>());
  rclcpp::shutdown();
  return 0;
}
