"""here4.launch.py — Launch the Here4 GPS DroneCAN bridge on CANable2.

bd-p47c: CANable2 freed by bd-wim1 (ESP32 moved to UART/USB) now used for
Here4 GPS at 1 Mbps DroneCAN/UAVCAN v0.

CAN setup (one-time, survives reboot via systemd-networkd or udev)
------------------------------------------------------------------
  # Option A — manual (quick test):
  sudo ip link set can0 down
  sudo ip link set can0 type can bitrate 1000000
  sudo ip link set can0 up

  # Option B — let the node do it (requires CAP_NET_ADMIN):
  ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true

  # Option C — systemd-networkd (/etc/systemd/network/80-can0.network):
  [Match]
  Name=can0
  [CAN]
  BitRate=1M

Published topics
----------------
  /gps/fix             sensor_msgs/NavSatFix     → navsat_transform_node (EKF)
  /gps/velocity        geometry_msgs/TwistWithCovarianceStamped
  /here4/fix           sensor_msgs/NavSatFix     (alias)
  /here4/imu           sensor_msgs/Imu
  /here4/mag           sensor_msgs/MagneticField
  /here4/baro          sensor_msgs/FluidPressure
  /here4/status        std_msgs/String JSON
  /here4/node_id       std_msgs/Int32

Subscribed topics
-----------------
  /rtcm                std_msgs/ByteMultiArray   RTCM corrections (NTRIP client)
  /rtcm_hex            std_msgs/String           hex-encoded RTCM (fallback)

Usage
-----
  # Default (interface already up):
  ros2 launch saltybot_dronecan_gps here4.launch.py

  # Bring up interface automatically (CAP_NET_ADMIN required):
  ros2 launch saltybot_dronecan_gps here4.launch.py bring_up_can:=true

  # Override interface (e.g. second CAN adapter):
  ros2 launch saltybot_dronecan_gps here4.launch.py can_interface:=can1

  # Pin to specific Here4 node ID (skip auto-detect):
  ros2 launch saltybot_dronecan_gps here4.launch.py node_id_filter:=10

System dependency
-----------------
  pip install dronecan          # DroneCAN/UAVCAN v0 Python library
  apt install can-utils         # optional: candump, cansend for debugging
"""

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description() -> LaunchDescription:
    pkg_share   = get_package_share_directory("saltybot_dronecan_gps")
    params_file = os.path.join(pkg_share, "config", "here4_params.yaml")

    args = [
        DeclareLaunchArgument(
            "can_interface",
            default_value="can0",
            description="SocketCAN interface (CANable2 @ 1 Mbps)",
        ),
        DeclareLaunchArgument(
            "bring_up_can",
            default_value="false",
            description="Bring up can_interface via ip link (requires CAP_NET_ADMIN)",
        ),
        DeclareLaunchArgument(
            "node_id_filter",
            default_value="0",
            description="DroneCAN node ID of Here4 (0=auto-detect from first Fix2)",
        ),
        DeclareLaunchArgument(
            "local_node_id",
            default_value="126",
            description="DroneCAN node ID for this Orin (must be unique on bus)",
        ),
    ]

    node = Node(
        package="saltybot_dronecan_gps",
        executable="here4_node",
        name="here4_can_node",
        output="screen",
        parameters=[
            params_file,
            {
                "can_interface":  LaunchConfiguration("can_interface"),
                "bring_up_can":   LaunchConfiguration("bring_up_can"),
                "node_id_filter": LaunchConfiguration("node_id_filter"),
                "local_node_id":  LaunchConfiguration("local_node_id"),
            },
        ],
    )

    return LaunchDescription([*args, node])