from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition 
from launch.substitutions import Command, LaunchConfiguration, IfElseSubstitution
from launch_ros.actions import Node, LifecycleNode
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():

    pkg_share = FindPackageShare("").find('toid_control')
    params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
    default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'rviz.rviz')

    description_pkg_share = FindPackageShare("").find('toid_bot_description')
    default_model_path = os.path.join(
        description_pkg_share, 
        'src', 
        'toid_bot_description.urdf'
    )

    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 use mock controller" 
    )

    odom_broadcast = Node(
        package='toid_odometry',
        executable='toid_odometry',
        name='map_to_odom_broadcaster',
        emulate_tty=True,
        parameters=[{'mock_odom': use_mock}],
    )


    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        name='robot_state_publisher',
        output='screen',
        emulate_tty=True,
        parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
    )

    controller_manager = Node(
        package='controller_manager',
        executable='ros2_control_node',
        output='screen',
        emulate_tty=True,
        parameters=[params],
        arguments=['--ros-args', '--log-level', 'warn']
    )

    joint_state_broadcaster = Node(
        package='controller_manager',
        executable='spawner',
        output='screen',
        emulate_tty=True,
        arguments=[
            "joint_state_broadcaster",
            '--ros-args', '--log-level', 'warn'
            ]
    )

    velocity_controller = Node(
        package='controller_manager',
        executable='spawner',
        output='screen',
        emulate_tty=True,
        arguments=[
            "velocity_controller",
            "--inactive",
            "-p",
            params,
            '--ros-args', '--log-level', 'warn'
            ],
    )


    diffbot_base_controller = Node(
        package='controller_manager',
        executable='spawner',
        output='both',
        emulate_tty=True,
        arguments=[
            "diffdrive_controller",
            "-p",
            params,
            "--controller-ros-args",
            "-r diffdrive_controller/cmd_vel:=/cmd_vel",
            "--controller-ros-args",
            "-r diffdrive_controller/odom:=/odom",
            "--controller-ros-args",
            IfElseSubstitution(use_mock, 
                "--param odom_frame_id:=odom", 
                "--param odom_frame_id:=odom_expected"
            ),
            "--controller-ros-args",
            IfElseSubstitution(use_mock, 
                "--param enable_odom_tf:=true", 
                "--param enable_odom_tf:=false"
            ),
            '--ros-args', '--log-level', 'warn'
        ]
    )

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        emulate_tty=True,
        arguments=['-d', default_rviz_config_path, 
                   '--ros-args', '--log-level', 'warn'
                   ],
        condition=IfCondition(visualize)
    )

    return LaunchDescription([
        visualize_arg,
        use_mock_arg,
        odom_broadcast,
        robot_state_publisher,
        controller_manager,
        joint_state_broadcaster,
        diffbot_base_controller,
        velocity_controller,
        rviz_node
    ])
