"""
cellular.launch.py — Launch all saltybot_cellular nodes.

Starts three nodes:
  gps_driver_node      — NMEA GPS → /gps/fix, /gps/vel
  cellular_manager_node — AT modem monitor → /cellular/status, /cellular/rssi
  mqtt_bridge_node      — ROS2 ↔ MQTT relay over cellular

Prerequisites:
  1. SIM7600X HAT connected via USB → /dev/ttyUSB0-2 must exist
  2. Nano-SIM with active data plan inserted
  3. NetworkManager profile "saltybot-cellular" configured (for nmcli method):
       nmcli connection add type gsm ifname '*' con-name saltybot-cellular apn <your-apn>

Usage:
  # Defaults (all params from cellular_params.yaml):
  ros2 launch saltybot_cellular cellular.launch.py

  # GPS only (skip modem + MQTT):
  ros2 launch saltybot_cellular cellular.launch.py \
      launch_cellular_manager:=false launch_mqtt_bridge:=false

  # Custom MQTT broker:
  ros2 launch saltybot_cellular cellular.launch.py mqtt_broker:=192.168.1.100

  # Override params file:
  ros2 launch saltybot_cellular cellular.launch.py \
      params_file:=/path/to/my_cellular_params.yaml

GPS topics:
  /gps/fix              (sensor_msgs/NavSatFix)
  /gps/vel              (geometry_msgs/TwistStamped)

Cellular topics:
  /cellular/status      (diagnostic_msgs/DiagnosticArray)
  /cellular/rssi        (std_msgs/Int32)
  /cellular/connected   (std_msgs/Bool)

MQTT relay (ROS2→MQTT):
  saltybot/imu, saltybot/gps/fix, saltybot/gps/vel, saltybot/uwb/ranges,
  saltybot/person, saltybot/cellular

MQTT relay (MQTT→ROS2):
  saltybot/cmd → /saltybot/cmd, saltybot/estop → /saltybot/estop
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def _launch_nodes(context, *args, **kwargs):
    params_file = LaunchConfiguration("params_file").perform(context)

    def _b(name):
        return LaunchConfiguration(name).perform(context).lower() == "true"

    def _s(name):
        return LaunchConfiguration(name).perform(context)

    def _f(name):
        return float(LaunchConfiguration(name).perform(context))

    def _i(name):
        return int(LaunchConfiguration(name).perform(context))

    gps_params = {
        "gps_port":        _s("gps_port"),
        "at_port":         _s("at_port"),
        "baud_rate":       _i("baud_rate"),
        "publish_rate":    _f("gps_publish_rate"),
        "frame_id":        _s("frame_id"),
        "gps_init_enable": _b("gps_init_enable"),
        "gps_init_cmd":    _s("gps_init_cmd"),
        "gps_accuracy":    _f("gps_accuracy"),
    }

    cellular_params = {
        "at_port":             _s("at_port"),
        "baud_rate":           _i("baud_rate"),
        "poll_rate":           _f("cellular_poll_rate"),
        "connection_method":   _s("connection_method"),
        "nmcli_profile":       _s("nmcli_profile"),
        "apn":                 _s("apn"),
        "reconnect_interval":  _f("reconnect_interval"),
    }

    mqtt_params = {
        "mqtt_broker":    _s("mqtt_broker"),
        "mqtt_port":      _i("mqtt_port"),
        "mqtt_user":      _s("mqtt_user"),
        "mqtt_password":  _s("mqtt_password"),
        "topic_prefix":   _s("topic_prefix"),
        "tls_enable":     _b("tls_enable"),
        "tls_ca_cert":    _s("tls_ca_cert"),
        "imu_rate":       _f("imu_rate"),
        "gps_rate":       _f("gps_rate"),
        "person_rate":    _f("person_rate"),
        "uwb_rate":       _f("uwb_rate"),
    }

    def _node_params(inline):
        return [params_file, inline] if params_file else [inline]

    nodes = [
        Node(
            package="saltybot_cellular",
            executable="gps_driver_node",
            name="gps_driver",
            output="screen",
            parameters=_node_params(gps_params),
        ),
    ]

    if _b("launch_cellular_manager"):
        nodes.append(Node(
            package="saltybot_cellular",
            executable="cellular_manager_node",
            name="cellular_manager",
            output="screen",
            parameters=_node_params(cellular_params),
        ))

    if _b("launch_mqtt_bridge"):
        nodes.append(Node(
            package="saltybot_cellular",
            executable="mqtt_bridge_node",
            name="mqtt_bridge",
            output="screen",
            parameters=_node_params(mqtt_params),
        ))

    return nodes


def generate_launch_description():
    pkg_share     = get_package_share_directory("saltybot_cellular")
    default_params = os.path.join(pkg_share, "config", "cellular_params.yaml")

    return LaunchDescription([
        DeclareLaunchArgument(
            "params_file", default_value=default_params,
            description="Full path to cellular_params.yaml (overrides inline args)"),

        # Node enable/disable
        DeclareLaunchArgument(
            "launch_cellular_manager", default_value="true",
            description="Launch cellular_manager_node"),
        DeclareLaunchArgument(
            "launch_mqtt_bridge", default_value="true",
            description="Launch mqtt_bridge_node"),

        # GPS driver
        DeclareLaunchArgument("gps_port",        default_value="/dev/ttyUSB2"),
        DeclareLaunchArgument("at_port",          default_value="/dev/ttyUSB0"),
        DeclareLaunchArgument("baud_rate",        default_value="115200"),
        DeclareLaunchArgument("gps_publish_rate", default_value="1.0",
                              description="GPS publish rate (Hz)"),
        DeclareLaunchArgument("frame_id",         default_value="gps"),
        DeclareLaunchArgument("gps_init_enable",  default_value="true",
                              description="Send AT+CGPS=1 on startup"),
        DeclareLaunchArgument("gps_init_cmd",     default_value="AT+CGPS=1"),
        DeclareLaunchArgument("gps_accuracy",     default_value="2.5",
                              description="GPS CEP accuracy at HDOP=1 (m)"),

        # Cellular manager
        DeclareLaunchArgument("cellular_poll_rate",  default_value="0.2",
                              description="AT poll rate (Hz)"),
        DeclareLaunchArgument("connection_method",   default_value="nmcli",
                              description="nmcli | pppd | none"),
        DeclareLaunchArgument("nmcli_profile",       default_value="saltybot-cellular"),
        DeclareLaunchArgument("apn",                 default_value=""),
        DeclareLaunchArgument("reconnect_interval",  default_value="30.0"),

        # MQTT bridge
        DeclareLaunchArgument("mqtt_broker",   default_value="mqtt.saltylab.local"),
        DeclareLaunchArgument("mqtt_port",     default_value="1883"),
        DeclareLaunchArgument("mqtt_user",     default_value=""),
        DeclareLaunchArgument("mqtt_password", default_value=""),
        DeclareLaunchArgument("topic_prefix",  default_value="saltybot"),
        DeclareLaunchArgument("tls_enable",    default_value="false"),
        DeclareLaunchArgument("tls_ca_cert",   default_value=""),
        DeclareLaunchArgument("imu_rate",      default_value="5.0"),
        DeclareLaunchArgument("gps_rate",      default_value="1.0"),
        DeclareLaunchArgument("person_rate",   default_value="2.0"),
        DeclareLaunchArgument("uwb_rate",      default_value="1.0"),

        OpaqueFunction(function=_launch_nodes),
    ])