from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition 
from launch.substitutions import  LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():
    nav_pkg_share = FindPackageShare("").find('toid_navigation')
    control_pkg_share = FindPackageShare("").find('toid_control')
    params = os.path.join(nav_pkg_share, 'params', 'toid_general_params.yaml')
    map = os.path.join(nav_pkg_share, 'maps', 'mapb2_5cm.yaml')
    ctrl_launch_dir = os.path.join(control_pkg_share, 'launch')
    default_rviz_config_path = os.path.join(nav_pkg_share, 'rviz', 'nav2.rviz')
    bt_path = os.path.join(nav_pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml")
    lattice_path = os.path.join(nav_pkg_share, "params", "output.json")

    visualize = LaunchConfiguration("visualize")
    visualize_arg = DeclareLaunchArgument(
        'visualize',
        default_value='False',
        description="Whether to launch rviz2" 
    )

    use_mock = LaunchConfiguration("use_mock")
    use_mock_arg = DeclareLaunchArgument(
        'use_mock',
        default_value='True',
        description="Whether to launch rviz2" 
    )

    run_nodes = LaunchConfiguration("run_nodes")
    run_nodes_arg = DeclareLaunchArgument(
        'run_nodes',
        default_value='True',
        description="Whether to launch rviz2" 
    )
    
    toid_control = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(ctrl_launch_dir, 'toid.launch.py')
        ),
        launch_arguments={
            'visualize': 'False',
            'use_mock': use_mock
        }.items(),
        condition=IfCondition(run_nodes),
    )

    map_server = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'yaml_filename': map}],
        condition=IfCondition(run_nodes),
    )

    planner_server = Node(
        package='nav2_planner',
        executable='planner_server',
        name='planner_server',
        output='screen',
        parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params],
        condition=IfCondition(run_nodes),
    )

    controller_server = Node(
        package='nav2_controller',
        executable='controller_server',
        name='controller_server',
        output='screen',
        parameters=[params],
        condition=IfCondition(run_nodes),
    )

    bt_navigator = Node(
        package='nav2_bt_navigator',
        executable='bt_navigator',
        name='bt_navigator',
        output='screen',
        parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params],
        condition=IfCondition(run_nodes),
    )

# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml"

    behavior_server = Node(
        package='nav2_behaviors',
        executable='behavior_server',
        name='behavior_server',
        output='screen',
        parameters=[params],
        condition=IfCondition(run_nodes),
    )

    lifecycle_manager_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager',
        output='screen',
        parameters=[params],
        condition=IfCondition(run_nodes),
    )

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', default_rviz_config_path],
        condition=IfCondition(visualize)
    )

    return LaunchDescription([
        visualize_arg,
        use_mock_arg,
        run_nodes_arg,
        rviz_node,
        map_server,
        bt_navigator,
        behavior_server,
        planner_server,
        controller_server,
        lifecycle_manager_node,
        toid_control
    ])