// 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 <atomic>
#include <cmath>
#include <cstring>
#include <mutex>
#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;
}

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);

    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());

    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);

      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;
    }
  }

  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;
    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 | lat=%.7f lon=%.7f alt=%.2f hσ=%.3fm nSV=%u",
                  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",
                  lat, lon, m.altitude, m.h_acc, m.num_sv);
      last_type_ = pos_type;
    }
  }

  // ── NTRIP ───────────────────────────────────────────────────────────────
  void ntrip_loop() {
    // 첫 유효 GGA 대기(VRS 부트스트랩)
    while (running_ && rclcpp::ok() && latest_gga().empty())
      std::this_thread::sleep_for(std::chrono::milliseconds(200));

    auto last_send = std::chrono::steady_clock::now();
    while (running_ && rclcpp::ok()) {
      if (ntrip_fd_ < 0) {
        if (!ntrip_connect()) { std::this_thread::sleep_for(std::chrono::seconds(2)); continue; }
        RCLCPP_INFO(get_logger(), "NTRIP 접속: %s:%d/%s", ntrip_host_.c_str(), ntrip_port_, ntrip_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 주입
      } else if (n == 0) {
        RCLCPP_WARN(get_logger(), "NTRIP 연결 종료 → 재접속");
        ::close(ntrip_fd_); ntrip_fd_ = -1; continue;
      }
      // VRS 유지용 GGA 주기 재전송
      auto now = std::chrono::steady_clock::now();
      if (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;
      }
    }
  }

  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 /" + ntrip_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")) { ::close(fd); return false; }
    std::string g = latest_gga();
    if (!g.empty()) ::send(fd, g.data(), g.size(), MSG_NOSIGNAL);
    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_;
  int serial_baud_{}, ntrip_port_{};
  bool use_ntrip_{}, fixed_only_{}, inflate_cov_{};
  double gga_period_{}, max_corr_age_{};

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

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