"""here4_node.py — ROS2 node for Here4 GPS via DroneCAN/UAVCAN v0 on CANable2.

bd-p47c: CANable2 (freed from ESP32 BALANCE comms by bd-wim1) is now used
for Here4 GPS at 1 Mbps DroneCAN.

CAN setup (run once before launching, or use bring_up_can:=true arg)
-----------------------------------------------------------------------
  sudo ip link set can0 down
  sudo ip link set can0 type can bitrate 1000000
  sudo ip link set can0 up

DroneCAN messages received from Here4
--------------------------------------
  uavcan.equipment.gnss.Fix2                 DTID 1063   ~5 Hz
  uavcan.equipment.gnss.Auxiliary            DTID 1060   ~5 Hz
  uavcan.equipment.ahrs.RawIMU               DTID 1003  ~50 Hz
  uavcan.equipment.ahrs.MagneticFieldStrength2 DTID 1001 ~10 Hz
  uavcan.equipment.air_data.StaticPressure   DTID 1028  ~10 Hz
  uavcan.equipment.air_data.StaticTemperature DTID 1029  ~10 Hz
  uavcan.protocol.NodeStatus                 DTID 341    ~1 Hz

DroneCAN messages transmitted to Here4
----------------------------------------
  uavcan.equipment.gnss.RTCMStream           DTID 1062   on /rtcm input

ROS2 topics published
----------------------
  /gps/fix              sensor_msgs/NavSatFix     5 Hz — feeds navsat_transform_node
  /gps/velocity         geometry_msgs/TwistWithCovarianceStamped  5 Hz
  /here4/fix            sensor_msgs/NavSatFix     alias of /gps/fix
  /here4/imu            sensor_msgs/Imu          50 Hz
  /here4/mag            sensor_msgs/MagneticField 10 Hz
  /here4/baro           sensor_msgs/FluidPressure 10 Hz
  /here4/status         std_msgs/String JSON       1 Hz (node ID, health, fix quality)
  /here4/node_id        std_msgs/Int32             node ID once discovered

ROS2 topics subscribed
-----------------------
  /rtcm                 std_msgs/ByteMultiArray   — RTCM corrections (from NTRIP client)
                        or std_msgs/String (hex-encoded, fallback)

Parameters
----------
  can_interface        str    SocketCAN interface name        default: can0
  local_node_id        int    This node's DroneCAN ID (1-127) default: 126
  bitrate              int    CAN bitrate, bps               default: 1000000
  bring_up_can         bool   Bring up can_interface via ip   default: false
  node_id_filter       int    Specific Here4 node ID (0=auto) default: 0
  fix_frame_id         str    Frame ID for NavSatFix          default: gps
  imu_frame_id         str    Frame ID for Imu                default: here4_imu
"""

from __future__ import annotations

import json
import math
import subprocess
import threading
import time
from typing import Any, Optional

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistWithCovarianceStamped
from sensor_msgs.msg import FluidPressure, Imu, MagneticField, NavSatFix, NavSatStatus
from std_msgs.msg import ByteMultiArray, Int32, String

from .dronecan_parser import (
    FIX_3D,
    FIX_NO_FIX,
    chunk_rtcm,
    parse_fix2,
    parse_mag,
    parse_raw_imu,
    parse_static_pressure,
    parse_static_temperature,
    parse_node_status,
)

# Gauss → Tesla conversion
_GA_TO_T = 1e-4

# NavSatStatus mapping from DroneCAN fix type
_FIX_TO_NAV_STATUS = {
    0: NavSatStatus.STATUS_NO_FIX,   # NO_FIX
    1: NavSatStatus.STATUS_NO_FIX,   # TIME_ONLY
    2: NavSatStatus.STATUS_FIX,      # 2D_FIX
    3: NavSatStatus.STATUS_FIX,      # 3D_FIX  (upgraded to SBAS/RTK when applicable)
}

# Here4 supports GPS+GLONASS+BeiDou+Galileo
_SERVICE_FLAGS = (NavSatStatus.SERVICE_GPS | NavSatStatus.SERVICE_GLONASS |
                  NavSatStatus.SERVICE_GALILEO | NavSatStatus.SERVICE_COMPASS)

# RTCM injection sequence counter (per-session, wraps at 65535)
_rtcm_seq = 0
_rtcm_lock = threading.Lock()


def _bring_up_can(iface: str, bitrate: int, logger) -> bool:
    """Bring up the SocketCAN interface at the requested bitrate."""
    try:
        subprocess.run(["ip", "link", "set", iface, "down"],          check=False)
        subprocess.run(["ip", "link", "set", iface, "type", "can",
                        "bitrate", str(bitrate)],                      check=True)
        subprocess.run(["ip", "link", "set", iface, "up"],             check=True)
        logger.info(f"SocketCAN {iface} up at {bitrate} bps")
        return True
    except subprocess.CalledProcessError as exc:
        logger.error(f"Failed to bring up {iface}: {exc} — check sudo/udev rules")
        return False


class Here4CanNode(Node):
    """DroneCAN bridge publishing Here4 GPS, IMU, mag, and baro to ROS2."""

    def __init__(self) -> None:
        super().__init__("here4_can_node")

        # ── Parameters ────────────────────────────────────────────────────
        self.declare_parameter("can_interface",    "can0")
        self.declare_parameter("local_node_id",    126)
        self.declare_parameter("bitrate",          1_000_000)
        self.declare_parameter("bring_up_can",     False)
        self.declare_parameter("node_id_filter",   0)       # 0 = auto-detect
        self.declare_parameter("fix_frame_id",     "gps")
        self.declare_parameter("imu_frame_id",     "here4_imu")

        self._iface        = self.get_parameter("can_interface").value
        self._local_nid    = self.get_parameter("local_node_id").value
        self._bitrate      = self.get_parameter("bitrate").value
        self._bring_up     = self.get_parameter("bring_up_can").value
        self._nid_filter   = self.get_parameter("node_id_filter").value
        self._fix_frame    = self.get_parameter("fix_frame_id").value
        self._imu_frame    = self.get_parameter("imu_frame_id").value

        # ── State ─────────────────────────────────────────────────────────
        self._here4_nid: Optional[int] = self._nid_filter if self._nid_filter else None
        self._last_fix: Optional[dict] = None
        self._last_temp_k: float = 293.15   # 20 °C default until first reading
        self._dc_node = None                # dronecan node, set up after CAN is ready

        # ── Publishers ────────────────────────────────────────────────────
        self._pub_fix     = self.create_publisher(NavSatFix,
                                "/gps/fix",       10)
        self._pub_fix_h4  = self.create_publisher(NavSatFix,
                                "/here4/fix",     10)
        self._pub_vel     = self.create_publisher(TwistWithCovarianceStamped,
                                "/gps/velocity",  10)
        self._pub_imu     = self.create_publisher(Imu,
                                "/here4/imu",     10)
        self._pub_mag     = self.create_publisher(MagneticField,
                                "/here4/mag",     10)
        self._pub_baro    = self.create_publisher(FluidPressure,
                                "/here4/baro",    10)
        self._pub_status  = self.create_publisher(String,
                                "/here4/status",  10)
        self._pub_node_id = self.create_publisher(Int32,
                                "/here4/node_id", 10)

        # ── Subscriptions ─────────────────────────────────────────────────
        self.create_subscription(ByteMultiArray, "/rtcm",
                                 self._on_rtcm_bytes, 10)
        self.create_subscription(String, "/rtcm_hex",
                                 self._on_rtcm_hex, 10)

        # ── Bring up CAN if requested ─────────────────────────────────────
        if self._bring_up:
            _bring_up_can(self._iface, self._bitrate, self.get_logger())

        # ── Initialise DroneCAN node ──────────────────────────────────────
        self._init_dronecan()

        # ── Timers ────────────────────────────────────────────────────────
        # Poll DroneCAN at 200 Hz (5 ms) to drain the RX queue in real-time
        self.create_timer(0.005, self._spin_dronecan)
        # Status publish at 1 Hz
        self.create_timer(1.0,   self._publish_status)

    # ── DroneCAN initialisation ───────────────────────────────────────────

    def _init_dronecan(self) -> None:
        try:
            import dronecan  # type: ignore[import]
        except ImportError:
            self.get_logger().error(
                "dronecan package not found — install with: pip install dronecan\n"
                "Here4 node will not receive GPS data until dronecan is installed."
            )
            return

        try:
            self._dc_node = dronecan.make_node(
                f"socketcan:{self._iface}",
                node_id=self._local_nid,
                bitrate=self._bitrate,
            )
        except Exception as exc:
            self.get_logger().error(
                f"Failed to open DroneCAN on {self._iface}: {exc}\n"
                f"Verify: ip link show {self._iface}  and try bring_up_can:=true"
            )
            return

        # ── Register message handlers ──────────────────────────────────────
        dc = dronecan
        self._dc_node.add_handler(dc.uavcan.equipment.gnss.Fix2,
                                  self._on_fix2)
        self._dc_node.add_handler(dc.uavcan.equipment.ahrs.RawIMU,
                                  self._on_raw_imu)
        self._dc_node.add_handler(dc.uavcan.equipment.ahrs.MagneticFieldStrength2,
                                  self._on_mag)
        self._dc_node.add_handler(dc.uavcan.equipment.air_data.StaticPressure,
                                  self._on_pressure)
        self._dc_node.add_handler(dc.uavcan.equipment.air_data.StaticTemperature,
                                  self._on_temperature)
        self._dc_node.add_handler(dc.uavcan.protocol.NodeStatus,
                                  self._on_node_status)

        self.get_logger().info(
            f"DroneCAN node ready: {self._iface} @ {self._bitrate} bps "
            f"local_id={self._local_nid}"
        )

    # ── DroneCAN spin (called from 5 ms timer) ────────────────────────────

    def _spin_dronecan(self) -> None:
        if self._dc_node is None:
            return
        try:
            self._dc_node.spin(timeout=0)
        except Exception as exc:
            self.get_logger().warning(
                f"DroneCAN spin error: {exc}",
                throttle_duration_sec=5.0,
            )

    # ── Source node filter ────────────────────────────────────────────────

    def _accept(self, event: Any) -> bool:  # type: ignore[valid-type]
        """Return True if this event comes from the tracked Here4 node."""
        src = getattr(getattr(event, "transfer", None), "source_node_id", None)
        if src is None:
            return True  # unknown source — accept
        if self._here4_nid is None:
            return True  # auto-detect mode — accept any
        return src == self._here4_nid

    # ── DroneCAN message handlers ─────────────────────────────────────────

    def _on_fix2(self, event) -> None:
        src = getattr(getattr(event, "transfer", None), "source_node_id", None)

        # Auto-latch: first node that sends Fix2 becomes our Here4
        if self._here4_nid is None and src is not None:
            self._here4_nid = src
            self.get_logger().info(
                f"Here4 auto-discovered: DroneCAN node ID {src}"
            )
            nid_msg = Int32()
            nid_msg.data = src
            self._pub_node_id.publish(nid_msg)

        if not self._accept(event):
            return
        d = parse_fix2(event.message)
        if d is None:
            return
        self._last_fix = d
        stamp = self.get_clock().now().to_msg()
        self._publish_navsatfix(d, stamp)
        self._publish_velocity(d, stamp)

    def _on_raw_imu(self, event) -> None:
        if not self._accept(event):
            return
        d = parse_raw_imu(event.message)
        if d is None:
            return
        stamp = self.get_clock().now().to_msg()
        msg = Imu()
        msg.header.stamp    = stamp
        msg.header.frame_id = self._imu_frame

        # Orientation: not provided by DroneCAN RawIMU — signal unknown
        msg.orientation_covariance[0] = -1.0

        gx, gy, gz = d["gyro_xyz"]
        msg.angular_velocity.x = gx
        msg.angular_velocity.y = gy
        msg.angular_velocity.z = gz
        _fill_cov9(msg.angular_velocity_covariance, d["gyro_cov"])

        ax, ay, az = d["accel_xyz"]
        msg.linear_acceleration.x = ax
        msg.linear_acceleration.y = ay
        msg.linear_acceleration.z = az
        _fill_cov9(msg.linear_acceleration_covariance, d["accel_cov"])

        self._pub_imu.publish(msg)

    def _on_mag(self, event) -> None:
        if not self._accept(event):
            return
        d = parse_mag(event.message)
        if d is None:
            return
        stamp = self.get_clock().now().to_msg()
        msg = MagneticField()
        msg.header.stamp    = stamp
        msg.header.frame_id = self._imu_frame
        gx, gy, gz = d["field_ga"]
        msg.magnetic_field.x = gx * _GA_TO_T
        msg.magnetic_field.y = gy * _GA_TO_T
        msg.magnetic_field.z = gz * _GA_TO_T
        _fill_cov9(msg.magnetic_field_covariance,
                   [v * _GA_TO_T * _GA_TO_T for v in d["field_cov"]])
        self._pub_mag.publish(msg)

    def _on_pressure(self, event) -> None:
        if not self._accept(event):
            return
        d = parse_static_pressure(event.message)
        if d is None:
            return
        stamp = self.get_clock().now().to_msg()
        msg = FluidPressure()
        msg.header.stamp    = stamp
        msg.header.frame_id = self._imu_frame
        msg.fluid_pressure  = d["pressure_pa"]
        msg.variance        = d["pressure_variance"]
        self._pub_baro.publish(msg)

    def _on_temperature(self, event) -> None:
        if not self._accept(event):
            return
        d = parse_static_temperature(event.message)
        if d is None:
            return
        self._last_temp_k = d["temperature_k"]

    def _on_node_status(self, event) -> None:
        src = getattr(getattr(event, "transfer", None), "source_node_id", None)
        if src is None:
            return
        d = parse_node_status(event.message, src)
        if d is None:
            return

        # Auto-discovery: latch first node that sends GPS-relevant data
        # NodeStatus alone is not enough — we track whichever source_node_id
        # first delivered a Fix2 message; if none yet, log all nodes seen.
        if self._here4_nid is None:
            # Any node could be Here4 — we'll latch on first Fix2 source
            self.get_logger().debug(
                f"DroneCAN node {src} alive: health={d['health_label']} "
                f"mode={d['mode_label']} uptime={d['uptime_sec']}s"
            )
        elif src == self._here4_nid:
            self._last_node_status = d

    # ── NavSatFix publishing ─────────────────────────────────────────────

    def _publish_navsatfix(self, d: dict, stamp) -> None:
        fix_type = d["fix_type"]

        status = NavSatStatus()
        status.service = _SERVICE_FLAGS

        # Upgrade status for RTK: hdop < 0.5 and 3D fix → treat as SBAS/RTK
        if fix_type == FIX_3D:
            if d["hdop"] < 0.5:
                status.status = NavSatStatus.STATUS_GBAS_FIX   # RTK fixed
            elif d["hdop"] < 1.5:
                status.status = NavSatStatus.STATUS_SBAS_FIX   # SBAS/DGPS
            else:
                status.status = NavSatStatus.STATUS_FIX
        else:
            status.status = _FIX_TO_NAV_STATUS.get(fix_type, NavSatStatus.STATUS_NO_FIX)

        msg = NavSatFix()
        msg.header.stamp    = stamp
        msg.header.frame_id = self._fix_frame
        msg.status          = status
        msg.latitude        = d["lat_deg"]
        msg.longitude       = d["lon_deg"]
        msg.altitude        = d["alt_msl_m"]

        # Position covariance (diagonal from DroneCAN; full matrix if available)
        pos_cov = d["pos_cov"]
        msg.position_covariance_type = (
            NavSatFix.COVARIANCE_TYPE_FULL if any(pos_cov) else
            NavSatFix.COVARIANCE_TYPE_APPROXIMATED
        )
        if any(pos_cov):
            for i, v in enumerate(pos_cov):
                msg.position_covariance[i] = v
        else:
            # Approximate from HDOP/VDOP: σ_h ≈ hdop × 3m, σ_v ≈ vdop × 5m
            sh2 = (d["hdop"] * 3.0) ** 2
            sv2 = (d["vdop"] * 5.0) ** 2
            msg.position_covariance[0] = sh2   # east
            msg.position_covariance[4] = sh2   # north
            msg.position_covariance[8] = sv2   # up

        self._pub_fix.publish(msg)
        self._pub_fix_h4.publish(msg)

    def _publish_velocity(self, d: dict, stamp) -> None:
        vn, ve, vd = d["ned_vel"]
        msg = TwistWithCovarianceStamped()
        msg.header.stamp    = stamp
        msg.header.frame_id = self._fix_frame
        # NED → ENU: x=east=ve, y=north=vn, z=up=-vd
        msg.twist.twist.linear.x = ve
        msg.twist.twist.linear.y = vn
        msg.twist.twist.linear.z = -vd
        # Velocity covariance: approximate from HDOP
        vel_var = (d["hdop"] * 0.2) ** 2
        msg.twist.covariance[0]  = vel_var   # xx (east)
        msg.twist.covariance[7]  = vel_var   # yy (north)
        msg.twist.covariance[14] = (d["vdop"] * 0.3) ** 2  # zz (up)
        self._pub_vel.publish(msg)

    # ── Status publish (1 Hz) ────────────────────────────────────────────

    def _publish_status(self) -> None:
        fix = self._last_fix
        ns = getattr(self, "_last_node_status", None)

        payload: dict = {
            "here4_node_id":  self._here4_nid,
            "can_interface":  self._iface,
            "dc_node_ready":  self._dc_node is not None,
            "fix_type":       fix["fix_type"]    if fix else FIX_NO_FIX,
            "fix_label":      fix["fix_label"]   if fix else "NO_FIX",
            "sats_used":      fix["sats_used"]   if fix else 0,
            "hdop":           round(fix["hdop"], 2) if fix else 99.0,
            "lat_deg":        round(fix["lat_deg"],    7) if fix else 0.0,
            "lon_deg":        round(fix["lon_deg"],    7) if fix else 0.0,
            "alt_msl_m":      round(fix["alt_msl_m"],  2) if fix else 0.0,
            "temp_c":         round(self._last_temp_k - 273.15, 1),
            "node_health":    ns["health_label"] if ns else "UNKNOWN",
            "node_uptime_s":  ns["uptime_sec"]   if ns else 0,
        }
        msg = String()
        msg.data = json.dumps(payload)
        self._pub_status.publish(msg)

        # Also publish discovered node ID
        if self._here4_nid is not None:
            nid_msg = Int32()
            nid_msg.data = self._here4_nid
            self._pub_node_id.publish(nid_msg)

    # ── RTCM injection ────────────────────────────────────────────────────

    def _on_rtcm_bytes(self, msg: ByteMultiArray) -> None:
        """Receive RTCM bytes from /rtcm and inject via DroneCAN RTCMStream."""
        raw = bytes(msg.data)
        self._inject_rtcm(raw)

    def _on_rtcm_hex(self, msg: String) -> None:
        """Receive hex-encoded RTCM from /rtcm_hex (fallback topic)."""
        try:
            raw = bytes.fromhex(msg.data.strip())
        except ValueError as exc:
            self.get_logger().error(f"Bad /rtcm_hex: {exc}")
            return
        self._inject_rtcm(raw)

    def _inject_rtcm(self, raw: bytes) -> None:
        if self._dc_node is None or not raw:
            return

        global _rtcm_seq
        try:
            import dronecan  # type: ignore[import]
        except ImportError:
            return

        with _rtcm_lock:
            chunks = chunk_rtcm(raw, _rtcm_seq)
            _rtcm_seq = (_rtcm_seq + len(chunks)) & 0xFFFF

        for chunk in chunks:
            try:
                rtcm_msg = dronecan.uavcan.equipment.gnss.RTCMStream()
                rtcm_msg.sequence_id = chunk["sequence_id"]
                rtcm_msg.data = list(chunk["data"])
                self._dc_node.broadcast(rtcm_msg)
            except Exception as exc:
                self.get_logger().warning(
                    f"RTCM inject error (seq {chunk['sequence_id']}): {exc}",
                    throttle_duration_sec=2.0,
                )

    # ── Shutdown ──────────────────────────────────────────────────────────

    def destroy_node(self) -> None:
        if self._dc_node is not None:
            try:
                self._dc_node.close()
            except Exception:
                pass
        super().destroy_node()


# ── Covariance helper ─────────────────────────────────────────────────────────

def _fill_cov9(ros_cov, values) -> None:
    """Fill a 9-element ROS2 covariance array from a list."""
    for i, v in enumerate(values[:9]):
        ros_cov[i] = float(v)


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

def main(args=None) -> None:
    rclpy.init(args=args)
    node = Here4CanNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
