"""
gps_driver_node.py — SIM7600X GPS NMEA → ROS2 NavSatFix + TwistStamped.

Hardware: Waveshare SIM7600X 4G HAT (SIMCom SIM7600A-H)
  GNSS: GPS + GLONASS + BeiDou + Galileo, ±2.5m CEP
  GPS serial port: /dev/ttyUSB2 (NMEA output, 115200 baud)
  AT port: /dev/ttyUSB0 (used to start GNSS receiver: AT+CGPS=1)

Publishes:
  /gps/fix  (sensor_msgs/NavSatFix)        — position with covariance from HDOP
  /gps/vel  (geometry_msgs/TwistStamped)   — ground speed + course (from NMEA RMC)
  /diagnostics                             — fix status, satellites, HDOP

Startup sequence:
  1. Optional: open AT port → send AT+CGPS=1 to start GNSS receiver
  2. Open GPS serial port → read NMEA sentences in background thread
  3. Publish latest GGA + RMC data at publish_rate Hz
"""

import math
import threading
import time

import rclpy
from rclpy.node import Node
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import NavSatFix, NavSatStatus

try:
    import serial
    _SERIAL_OK = True
except ImportError:
    _SERIAL_OK = False


# ── Pure NMEA parsing functions ───────────────────────────────────────────────
# Extracted here for unit tests — no ROS2 imports needed.

GPS_CEP_ACCURACY = 2.5   # metres at HDOP=1 (SIM7600A-H spec)
KNOTS_TO_MS      = 0.514444


def nmea_checksum_valid(sentence: str) -> bool:
    """Validate NMEA checksum. Sentence must include the leading '$'."""
    if '*' not in sentence:
        return False
    data, checksum_str = sentence[1:].rsplit('*', 1)
    try:
        expected = int(checksum_str.strip()[:2], 16)
    except ValueError:
        return False
    computed = 0
    for ch in data:
        computed ^= ord(ch)
    return computed == expected


def nmea_dm_to_degrees(dm_str: str, direction: str) -> float:
    """
    Convert NMEA DDMM.MMMM or DDDMM.MMMM to decimal degrees.

    NMEA format: degrees are everything up to the last two digits before the
    decimal point (minutes field is always MM.fractional).
    """
    if not dm_str:
        return None
    dot = dm_str.index('.')
    deg = float(dm_str[:dot - 2])
    mins = float(dm_str[dot - 2:])
    decimal = deg + mins / 60.0
    if direction in ('S', 'W'):
        decimal = -decimal
    return decimal


def parse_gga(sentence: str) -> dict:
    """
    Parse NMEA GGA sentence (any talker: GP/GN/GL/GA/GB).

    Returns dict with keys:
      latitude, longitude, altitude, fix_quality, num_satellites, hdop, utc_time
    Returns None on parse error or no-fix (fix_quality == 0).

    GGA field indices (0-based after splitting by comma):
      0  Talker+GGA   1  UTC      2  Lat    3  N/S
      4  Lon          5  E/W      6  Fix    7  NumSV
      8  HDOP         9  AltMSL   10 M      11 GeoidSep
      12 M            13 Age      14 StaID*CS
    """
    raw = sentence.strip()
    if raw.startswith('$'):
        raw = raw[1:]
    if '*' in raw:
        raw = raw[:raw.index('*')]
    parts = raw.split(',')
    if len(parts) < 10:
        return None
    if not parts[0].endswith('GGA'):
        return None
    try:
        fix_quality = int(parts[6]) if parts[6] else 0
    except ValueError:
        return None
    if fix_quality == 0:
        return None
    lat = nmea_dm_to_degrees(parts[2], parts[3])
    lon = nmea_dm_to_degrees(parts[4], parts[5])
    if lat is None or lon is None:
        return None
    try:
        num_sats  = int(parts[7])   if parts[7]  else 0
        hdop      = float(parts[8]) if parts[8]  else 99.0
        alt_msl   = float(parts[9]) if parts[9]  else 0.0
        geoid_sep = float(parts[11]) if len(parts) > 11 and parts[11] else 0.0
    except (ValueError, IndexError):
        return None
    return {
        'latitude':         lat,
        'longitude':        lon,
        'altitude':         alt_msl + geoid_sep,   # WGS84 ellipsoidal height
        'fix_quality':      fix_quality,
        'num_satellites':   num_sats,
        'hdop':             hdop,
        'utc_time':         parts[1],
    }


def parse_rmc(sentence: str) -> dict:
    """
    Parse NMEA RMC sentence (any talker).

    Returns dict with keys:
      speed_mps, course_deg, valid, utc_time
    Returns None on parse error or void status (status == 'V').

    RMC field indices:
      0  Talker+RMC  1  UTC    2  Status(A/V)  3  Lat  4  N/S
      5  Lon         6  E/W    7  SpeedKnots   8  CourseDeg  9  Date
      10 MagVar      11 MagDir  12 Mode*CS
    """
    raw = sentence.strip()
    if raw.startswith('$'):
        raw = raw[1:]
    if '*' in raw:
        raw = raw[:raw.index('*')]
    parts = raw.split(',')
    if len(parts) < 9:
        return None
    if not parts[0].endswith('RMC'):
        return None
    if parts[2] != 'A':   # 'V' = void / no fix
        return None
    try:
        speed_knots = float(parts[7]) if parts[7] else 0.0
        course_deg  = float(parts[8]) if parts[8] else 0.0
    except ValueError:
        return None
    return {
        'speed_mps':   speed_knots * KNOTS_TO_MS,
        'course_deg':  course_deg,
        'valid':       True,
        'utc_time':    parts[1],
    }


def hdop_to_covariance(hdop: float, accuracy_m: float = GPS_CEP_ACCURACY) -> list:
    """
    Compute diagonal position covariance matrix (row-major 3×3) from HDOP.

    sigma_h = hdop * accuracy_m   (horizontal 1σ ≈ CEP * 1.2)
    sigma_v = sigma_h * 1.5       (vertical is typically 1.5× worse)
    """
    sigma_h = hdop * accuracy_m
    sigma_v = sigma_h * 1.5
    return [
        sigma_h**2, 0.0,        0.0,
        0.0,        sigma_h**2, 0.0,
        0.0,        0.0,        sigma_v**2,
    ]


def fix_quality_to_status(fix_quality: int) -> int:
    """Map NMEA GGA fix quality to NavSatStatus.status."""
    if fix_quality <= 0:
        return NavSatStatus.STATUS_NO_FIX   # -1
    if fix_quality == 2:
        return NavSatStatus.STATUS_SBAS_FIX  # 1 (DGPS)
    if fix_quality in (4, 5):
        return NavSatStatus.STATUS_GBAS_FIX  # 2 (RTK)
    return NavSatStatus.STATUS_FIX           # 0 (standard GPS)


def course_to_velocity_enu(speed_mps: float, course_deg: float) -> tuple:
    """
    Convert scalar ground speed + course-over-ground to ENU velocity components.

    Returns (v_east, v_north).
    Course: 0° = North, 90° = East (compass convention).
    """
    course_rad = math.radians(course_deg)
    v_north = speed_mps * math.cos(course_rad)
    v_east  = speed_mps * math.sin(course_rad)
    return v_east, v_north


# ── ROS2 Node ─────────────────────────────────────────────────────────────────

class GpsDriverNode(Node):

    def __init__(self):
        super().__init__("gps_driver")

        self.declare_parameter("gps_port",        "/dev/ttyUSB2")
        self.declare_parameter("at_port",         "/dev/ttyUSB0")
        self.declare_parameter("baud_rate",       115200)
        self.declare_parameter("publish_rate",    1.0)
        self.declare_parameter("frame_id",        "gps")
        self.declare_parameter("gps_init_enable", True)
        self.declare_parameter("gps_init_cmd",    "AT+CGPS=1")
        self.declare_parameter("at_init_timeout", 3.0)
        self.declare_parameter("gps_accuracy",    GPS_CEP_ACCURACY)

        p = {
            "gps_port":        self.get_parameter("gps_port").value,
            "at_port":         self.get_parameter("at_port").value,
            "baud_rate":       self.get_parameter("baud_rate").value,
            "publish_rate":    self.get_parameter("publish_rate").value,
            "frame_id":        self.get_parameter("frame_id").value,
            "gps_init_enable": self.get_parameter("gps_init_enable").value,
            "gps_init_cmd":    self.get_parameter("gps_init_cmd").value,
            "at_init_timeout": self.get_parameter("at_init_timeout").value,
            "gps_accuracy":    self.get_parameter("gps_accuracy").value,
        }
        self._p = p

        # Publishers
        self._fix_pub  = self.create_publisher(NavSatFix,     "/gps/fix",   10)
        self._vel_pub  = self.create_publisher(TwistStamped,  "/gps/vel",   10)
        self._diag_pub = self.create_publisher(DiagnosticArray, "/diagnostics", 10)

        # Latest parsed data (thread-safe via GIL for simple assignment)
        self._gga   = None   # dict from parse_gga
        self._rmc   = None   # dict from parse_rmc
        self._ser   = None
        self._running = False

        if not _SERIAL_OK:
            self.get_logger().error("pyserial not installed — GPS node inactive")
            return

        # Optional: send AT+CGPS=1 to start GNSS
        if p["gps_init_enable"]:
            self._send_at_init()

        # Start background serial reader
        self._running = True
        self._reader_thread = threading.Thread(
            target=self._reader_loop, daemon=True)
        self._reader_thread.start()

        # Publish timer
        self.create_timer(1.0 / p["publish_rate"], self._publish_cb)

        self.get_logger().info(
            f"GpsDriver ready  port={p['gps_port']} rate={p['publish_rate']}Hz")

    # ── AT init ───────────────────────────────────────────────────────────────

    def _send_at_init(self):
        """Open AT port, send GPS-start command, close."""
        try:
            with serial.Serial(
                    self._p["at_port"], self._p["baud_rate"],
                    timeout=self._p["at_init_timeout"]) as at:
                cmd = (self._p["gps_init_cmd"] + "\r\n").encode("ascii")
                at.write(cmd)
                resp = at.read_until(b"OK\r\n", size=256)
                if b"OK" in resp:
                    self.get_logger().info("GPS init OK (AT+CGPS=1)")
                else:
                    self.get_logger().warn(
                        f"GPS init: unexpected response: {resp!r}")
        except serial.SerialException as exc:
            self.get_logger().warn(
                f"GPS AT init failed ({exc}) — assuming GPS already running")

    # ── Background NMEA reader ─────────────────────────────────────────────────

    def _reader_loop(self):
        while self._running:
            try:
                with serial.Serial(
                        self._p["gps_port"], self._p["baud_rate"],
                        timeout=1.0) as ser:
                    self._ser = ser
                    self.get_logger().info(
                        f"GPS serial open: {self._p['gps_port']}")
                    while self._running:
                        line = ser.readline().decode("ascii", errors="replace").strip()
                        if not line:
                            continue
                        if not nmea_checksum_valid("$" + line.lstrip("$")):
                            continue
                        gga = parse_gga(line)
                        if gga:
                            self._gga = gga
                        rmc = parse_rmc(line)
                        if rmc:
                            self._rmc = rmc
            except serial.SerialException as exc:
                self._ser = None
                if self._running:
                    self.get_logger().warn(
                        f"GPS serial error ({exc}) — retrying in 3s")
                    time.sleep(3.0)
            except Exception as exc:
                self.get_logger().error(f"GPS reader exception: {exc}")
                time.sleep(3.0)

    # ── Publish callback ──────────────────────────────────────────────────────

    def _publish_cb(self):
        now = self.get_clock().now().to_msg()
        gga = self._gga
        rmc = self._rmc

        # NavSatFix
        fix_msg = NavSatFix()
        fix_msg.header.stamp    = now
        fix_msg.header.frame_id = self._p["frame_id"]
        if gga:
            fix_msg.status.service  = (NavSatStatus.SERVICE_GPS |
                                       NavSatStatus.SERVICE_GLONASS)
            fix_msg.status.status   = fix_quality_to_status(gga["fix_quality"])
            fix_msg.latitude        = gga["latitude"]
            fix_msg.longitude       = gga["longitude"]
            fix_msg.altitude        = gga["altitude"]
            fix_msg.position_covariance = hdop_to_covariance(
                gga["hdop"], self._p["gps_accuracy"])
            fix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
        else:
            fix_msg.status.status  = NavSatStatus.STATUS_NO_FIX
            fix_msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
        self._fix_pub.publish(fix_msg)

        # TwistStamped (ground velocity)
        if rmc:
            vel_msg = TwistStamped()
            vel_msg.header.stamp    = now
            vel_msg.header.frame_id = "odom"   # ENU velocity in odom frame
            v_east, v_north = course_to_velocity_enu(
                rmc["speed_mps"], rmc["course_deg"])
            vel_msg.twist.linear.x  = v_east    # East
            vel_msg.twist.linear.y  = v_north   # North
            vel_msg.twist.angular.z = math.radians(rmc["course_deg"])  # heading
            self._vel_pub.publish(vel_msg)

        # Diagnostics
        diag_arr = DiagnosticArray()
        diag_arr.header.stamp = now
        ds = DiagnosticStatus()
        ds.name     = "GPS"
        ds.hardware_id = "SIM7600A-H"
        if gga:
            ds.level    = DiagnosticStatus.OK
            ds.message  = f"Fix quality {gga['fix_quality']}, {gga['num_satellites']} sats"
            ds.values   = [
                KeyValue(key="fix_quality",    value=str(gga["fix_quality"])),
                KeyValue(key="satellites",     value=str(gga["num_satellites"])),
                KeyValue(key="hdop",           value=f"{gga['hdop']:.1f}"),
                KeyValue(key="latitude",       value=f"{gga['latitude']:.6f}"),
                KeyValue(key="longitude",      value=f"{gga['longitude']:.6f}"),
                KeyValue(key="altitude_m",     value=f"{gga['altitude']:.1f}"),
            ]
        else:
            ds.level   = DiagnosticStatus.WARN
            ds.message = "No GPS fix"
        diag_arr.status = [ds]
        self._diag_pub.publish(diag_arr)

    def destroy_node(self):
        self._running = False
        super().destroy_node()


# ── Entry point ───────────────────────────────────────────────────────────────

def main(args=None):
    rclpy.init(args=args)
    node = GpsDriverNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == "__main__":
    main()
