"""
cellular_manager_node.py — SIM7600X 4G modem status monitor + connection manager.

Hardware: Waveshare SIM7600X 4G HAT (SIMCom SIM7600A-H)
  AT command port: /dev/ttyUSB0 (115200 baud)
  Data connection managed by NetworkManager (nmcli) or pppd

Publishes:
  /cellular/status  (diagnostic_msgs/DiagnosticArray) — rssi, network, connected
  /cellular/rssi    (std_msgs/Int32)                  — signal strength dBm (-113..0)
  /cellular/connected (std_msgs/Bool)                 — data connection active

Key AT commands used:
  AT+CSQ      → signal quality (rssi 0-31, ber)
  AT+CREG?    → network registration status
  AT+COPS?    → operator name
  AT+CGDCONT? → PDP context / APN

4G data connection management:
  connection_method = "nmcli": uses `nmcli connection up <profile>`
  connection_method = "pppd":  uses pppd with provided dial script
  connection_method = "none":  monitor only, no auto-connect
"""

import subprocess
import threading
import time

import rclpy
from rclpy.node import Node
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from std_msgs.msg import Bool, Int32

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


# ── Pure AT response parsing functions ────────────────────────────────────────

CREG_STATUS = {
    "0": "not_registered",
    "1": "registered_home",
    "2": "searching",
    "3": "denied",
    "4": "unknown",
    "5": "roaming",
}


def parse_csq(response: str) -> dict:
    """
    Parse AT+CSQ response.

    Expected response: '+CSQ: <rssi>,<ber>'
    rssi: 0-31 → -113 + 2*rssi dBm; 99 = unknown
    ber:  0-7 (bit error rate); 99 = unknown

    Returns dict with rssi_raw, rssi_dbm, ber, valid.
    """
    for line in response.splitlines():
        line = line.strip()
        if not line.startswith("+CSQ:"):
            continue
        parts = line[5:].strip().split(",")
        if len(parts) < 2:
            continue
        try:
            rssi_raw = int(parts[0].strip())
            ber      = int(parts[1].strip())
        except ValueError:
            continue
        if rssi_raw == 99:
            return {"rssi_raw": 99, "rssi_dbm": None, "ber": ber, "valid": False}
        return {
            "rssi_raw": rssi_raw,
            "rssi_dbm": -113 + 2 * rssi_raw,
            "ber":      ber,
            "valid":    True,
        }
    return {"rssi_raw": 99, "rssi_dbm": None, "ber": 99, "valid": False}


def parse_creg(response: str) -> dict:
    """
    Parse AT+CREG? response.

    Expected: '+CREG: <n>,<stat>' or '+CREG: <stat>'
    Returns dict with stat_raw, status_str, registered.
    """
    for line in response.splitlines():
        line = line.strip()
        if not line.startswith("+CREG:"):
            continue
        payload = line[6:].strip()
        parts   = payload.split(",")
        # Can be '<n>,<stat>' or just '<stat>'
        stat_str = parts[-1].strip()
        stat_str = stat_str.split()[0]   # strip trailing chars
        status   = CREG_STATUS.get(stat_str, "unknown")
        return {
            "stat_raw":   stat_str,
            "status_str": status,
            "registered": stat_str in ("1", "5"),
        }
    return {"stat_raw": "?", "status_str": "unknown", "registered": False}


def parse_cops(response: str) -> str:
    """
    Parse AT+COPS? response.

    Expected: '+COPS: <mode>,<format>,<oper>[,<AcT>]'
    Returns operator name string, or '' if not available.
    """
    for line in response.splitlines():
        line = line.strip()
        if not line.startswith("+COPS:"):
            continue
        parts = line[6:].strip().split(",")
        if len(parts) >= 3:
            return parts[2].strip().strip('"')
    return ""


def rssi_to_quality_label(rssi_dbm) -> str:
    """Human-readable signal quality label from RSSI dBm."""
    if rssi_dbm is None:
        return "no_signal"
    if rssi_dbm >= -70:
        return "excellent"
    if rssi_dbm >= -85:
        return "good"
    if rssi_dbm >= -100:
        return "fair"
    return "poor"


# ── AT command helper ─────────────────────────────────────────────────────────

def _at_command(ser, cmd: str, timeout: float = 2.0) -> str:
    """Send AT command, return response string (up to OK/ERROR)."""
    ser.reset_input_buffer()
    ser.write((cmd + "\r\n").encode("ascii"))
    deadline = time.monotonic() + timeout
    buf = ""
    while time.monotonic() < deadline:
        chunk = ser.read(ser.in_waiting or 1).decode("ascii", errors="replace")
        buf += chunk
        if "OK\r\n" in buf or "ERROR" in buf:
            break
    return buf


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

class CellularManagerNode(Node):

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

        self.declare_parameter("at_port",            "/dev/ttyUSB0")
        self.declare_parameter("baud_rate",          115200)
        self.declare_parameter("poll_rate",          0.2)      # Hz (every 5s)
        self.declare_parameter("at_timeout",         2.0)      # s per AT command
        self.declare_parameter("connection_method",  "nmcli")  # nmcli|pppd|none
        self.declare_parameter("nmcli_profile",      "saltybot-cellular")
        self.declare_parameter("apn",                "")
        self.declare_parameter("reconnect_interval", 30.0)     # s between reconnect tries

        p = {k: self.get_parameter(k).value for k in [
            "at_port", "baud_rate", "poll_rate", "at_timeout",
            "connection_method", "nmcli_profile", "apn", "reconnect_interval",
        ]}
        self._p = p

        # State
        self._csq  = {"rssi_raw": 99, "rssi_dbm": None, "ber": 99, "valid": False}
        self._creg = {"stat_raw": "?", "status_str": "unknown", "registered": False}
        self._operator  = ""
        self._connected = False
        self._last_reconnect = 0.0
        self._lock = threading.Lock()

        # Publishers
        self._status_pub  = self.create_publisher(DiagnosticArray, "/cellular/status", 10)
        self._rssi_pub    = self.create_publisher(Int32,           "/cellular/rssi",   10)
        self._conn_pub    = self.create_publisher(Bool,            "/cellular/connected", 10)

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

        # Background AT poll thread
        self._running = True
        self._poll_thread = threading.Thread(target=self._poll_loop, daemon=True)
        self._poll_thread.start()

        # Publish timer (same rate as poll)
        self.create_timer(1.0 / p["poll_rate"], self._publish_cb)

        self.get_logger().info(
            f"CellularManager ready  at_port={p['at_port']} "
            f"method={p['connection_method']}")

    # ── AT poll loop ──────────────────────────────────────────────────────────

    def _poll_loop(self):
        interval = 1.0 / self._p["poll_rate"]
        while self._running:
            try:
                with serial.Serial(
                        self._p["at_port"], self._p["baud_rate"],
                        timeout=self._p["at_timeout"] + 0.5) as ser:
                    self.get_logger().info(
                        f"AT port open: {self._p['at_port']}")
                    while self._running:
                        t0 = time.monotonic()
                        self._poll_once(ser)
                        elapsed = time.monotonic() - t0
                        if elapsed < interval:
                            time.sleep(interval - elapsed)
            except serial.SerialException as exc:
                if self._running:
                    self.get_logger().warn(
                        f"AT serial error ({exc}) — retrying in 5s")
                    time.sleep(5.0)

    def _poll_once(self, ser):
        """Query AT+CSQ, AT+CREG?, AT+COPS? and update state."""
        try:
            csq_resp  = _at_command(ser, "AT+CSQ",   self._p["at_timeout"])
            creg_resp = _at_command(ser, "AT+CREG?", self._p["at_timeout"])
            cops_resp = _at_command(ser, "AT+COPS?", self._p["at_timeout"])
        except Exception as exc:
            self.get_logger().warn(f"AT poll error: {exc}")
            return

        csq      = parse_csq(csq_resp)
        creg     = parse_creg(creg_resp)
        operator = parse_cops(cops_resp)
        connected = self._check_connectivity()

        with self._lock:
            self._csq      = csq
            self._creg     = creg
            self._operator = operator
            self._connected = connected

        # Auto-reconnect if data link is down
        if not connected and self._p["connection_method"] != "none":
            now = time.monotonic()
            if now - self._last_reconnect > self._p["reconnect_interval"]:
                self._last_reconnect = now
                self._attempt_reconnect()

    # ── Connectivity check ────────────────────────────────────────────────────

    def _check_connectivity(self) -> bool:
        """Return True if cellular data connection is active."""
        method = self._p["connection_method"]
        try:
            if method == "nmcli":
                result = subprocess.run(
                    ["nmcli", "-t", "-f", "GENERAL.STATE",
                     "con", "show", self._p["nmcli_profile"]],
                    capture_output=True, text=True, timeout=5.0)
                return "activated" in result.stdout.lower()
            if method == "pppd":
                result = subprocess.run(
                    ["ip", "link", "show", "ppp0"],
                    capture_output=True, text=True, timeout=3.0)
                return result.returncode == 0 and "UP" in result.stdout
        except Exception:
            pass
        return False

    def _attempt_reconnect(self):
        """Attempt to bring up cellular data connection."""
        method = self._p["connection_method"]
        self.get_logger().info(f"Attempting cellular reconnect ({method})")
        try:
            if method == "nmcli":
                subprocess.run(
                    ["nmcli", "connection", "up", self._p["nmcli_profile"]],
                    timeout=30.0, check=False)
            elif method == "pppd":
                subprocess.run(
                    ["systemctl", "restart", "pppd-cellular"],
                    timeout=15.0, check=False)
        except Exception as exc:
            self.get_logger().warn(f"Reconnect failed: {exc}")

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

    def _publish_cb(self):
        with self._lock:
            csq       = self._csq
            creg      = self._creg
            operator  = self._operator
            connected = self._connected

        now = self.get_clock().now().to_msg()

        # DiagnosticArray — full status
        ds = DiagnosticStatus()
        ds.name        = "Cellular"
        ds.hardware_id = "SIM7600A-H"
        ds.level       = DiagnosticStatus.OK if connected else DiagnosticStatus.WARN
        ds.message     = (
            f"{'Connected' if connected else 'Disconnected'} | "
            f"{operator or 'unknown'} | "
            f"{creg['status_str']} | "
            f"{rssi_to_quality_label(csq['rssi_dbm'])}"
        )
        ds.values = [
            KeyValue(key="connected",      value=str(connected)),
            KeyValue(key="registration",   value=creg["status_str"]),
            KeyValue(key="operator",       value=operator),
            KeyValue(key="rssi_raw",       value=str(csq["rssi_raw"])),
            KeyValue(key="rssi_dbm",       value=str(csq["rssi_dbm"] or "unknown")),
            KeyValue(key="signal_quality", value=rssi_to_quality_label(csq["rssi_dbm"])),
            KeyValue(key="ber",            value=str(csq["ber"])),
        ]
        arr = DiagnosticArray()
        arr.header.stamp = now
        arr.status = [ds]
        self._status_pub.publish(arr)

        # /cellular/rssi (Int32, dBm; 0 if unknown)
        rssi_msg = Int32()
        rssi_msg.data = csq["rssi_dbm"] if csq["rssi_dbm"] is not None else 0
        self._rssi_pub.publish(rssi_msg)

        # /cellular/connected (Bool)
        conn_msg = Bool()
        conn_msg.data = connected
        self._conn_pub.publish(conn_msg)

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


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

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


if __name__ == "__main__":
    main()
