from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    # ── origincar_bridge params ──
    declare_http_host = DeclareLaunchArgument(
        "http_host", default_value="192.168.64.1",
        description="simulation host IP (macOS)")
    declare_http_port = DeclareLaunchArgument(
        "http_port", default_value="8765")

    # ── car_nav_lite params ──
    declare_map_file = DeclareLaunchArgument(
        "map_file", default_value="/home/cyy/sim_map.pgm")
    declare_lookahead = DeclareLaunchArgument(
        "lookahead", default_value="0.8")
    declare_test_mode = DeclareLaunchArgument(
        "test_mode", default_value="false")
    declare_foxglove = DeclareLaunchArgument(
        "foxglove", default_value="true",
        description="also launch foxglove_bridge")

    return LaunchDescription([
        declare_http_host, declare_http_port,
        declare_map_file, declare_lookahead, declare_test_mode, declare_foxglove,

        # ── 1. HTTP→ROS2 bridge (sim ↔ nav) ──
        Node(
            package="origincar_bridge",
            executable="bridge_node",
            name="origincar_bridge",
            output="screen",
            parameters=[{
                "http_host": LaunchConfiguration("http_host"),
                "http_port": LaunchConfiguration("http_port"),
            }],
        ),

        # ── 2. Static TF: base_link → laser ──
        Node(
            package="tf2_ros",
            executable="static_transform_publisher",
            name="tf_laser",
            arguments=["0", "0", "0.12", "0", "0", "0", "base_link", "laser"],
        ),

        # ── 3. Main navigation node ──
        Node(
            package="car_nav_lite",
            executable="nav_lite_node",
            name="nav_lite_node",
            output="screen",
            parameters=[{
                "map_file": LaunchConfiguration("map_file"),
                "lookahead": LaunchConfiguration("lookahead"),
                "test_mode": LaunchConfiguration("test_mode"),
            }],
        ),

        # ── 4. Foxglove bridge (for visualization) ──
        Node(
            package="foxglove_bridge",
            executable="foxglove_bridge",
            name="foxglove_bridge",
            output="screen",
            parameters=[{"port": 8766}],
            condition=IfCondition(LaunchConfiguration("foxglove")),
        ),
    ])